feat(pinocchio models)

environment.yml

@@ -14,6 +14,8 @@ dependencies:
 - imageio
 - imageio-ffmpeg
 - opensim
+- pinocchio
+- pycollada
 # from pip
 # pip install trc-data-reader
 

examples/pinocchio/from_pinocchio_model.py

@@ -0,0 +1,59 @@
+"""
+Example of using Pinocchio model with pyorerun.
+
+This example demonstrates how to load and animate a URDF model (Baxter robot) using the Pinocchio interface.
+"""
+
+import numpy as np
+from pathlib import Path
+
+# Check if pinocchio is available
+try:
+    import pinocchio as pin
+    from pyorerun import PhaseRerun, PinocchioModel
+except ImportError as e:
+    print(f"Error: {e}")
+    print("Please install pinocchio: pip install pin")
+    exit(1)
+
+
+def main():
+    # Use the Baxter robot model from the examples
+    # Get the path relative to this script
+    script_dir = Path(__file__).parent
+    model_path = script_dir / "urdf" / "baxter_local.urdf"
+
+    if not model_path.exists():
+        print(f"Model file not found: {model_path}")
+        return
+
+    # Load the Pinocchio model
+    print(f"Loading model from: {model_path}")
+    pinocchio_model = PinocchioModel(str(model_path))
+
+    # Create a simple animation
+    nb_frames = 200
+    nb_seconds = 2
+    t_span = np.linspace(0, nb_seconds, nb_frames)
+
+    # Generate some generalized coordinates
+    # For a standing position, we need to generate appropriate q values
+    q = np.zeros((pinocchio_model.nb_q, nb_frames))
+
+    # Add some simple motion (sinusoidal movement on some joints)
+    for i in range(min(3, pinocchio_model.nb_q)):
+        q[i, :] = 0.1 * np.sin(2 * np.pi * t_span * (i + 1))
+
+    # Create the visualization
+    print(f"Model name: {pinocchio_model.name}")
+    print(f"Number of DoFs: {pinocchio_model.nb_q}")
+    print(f"Number of segments: {pinocchio_model.nb_segments}")
+    print(f"Number of markers: {pinocchio_model.nb_markers}")
+
+    viz = PhaseRerun(t_span)
+    viz.add_animated_model(pinocchio_model, q)
+    viz.rerun("pinocchio_model_example")
+
+
+if __name__ == "__main__":
+    main()

examples/pinocchio/meshes/base/pedestal_link_collision.DAE

@@ -0,0 +1,62 @@
+<?xml version="1.0" encoding="utf-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+  <asset>
+    <contributor>
+      <author>Blender User</author>
+      <authoring_tool>Blender 2.67.0 r57141</authoring_tool>
+    </contributor>
+    <created>2013-08-05T13:45:40</created>
+    <modified>2013-08-05T13:45:40</modified>
+    <unit name="meter" meter="1"/>
+    <up_axis>Z_UP</up_axis>
+  </asset>
+  <library_images/>
+  <library_geometries>
+    <geometry id="PEDESTAL-L-mesh" name="PEDESTAL-L">
+      <mesh>
+        <source id="PEDESTAL-L-mesh-positions">
+          <float_array id="PEDESTAL-L-mesh-positions-array" count="315">0 -0.1922612 -0.7516 -0.2904722 -0.2632086 -0.7516 -0.2904722 -0.1922612 -0.7516 0 0.1922612 -0.7516 0.2440557 0.2558969 -0.7516 0.4170677 0.218715 -0.7516 0.4170677 -0.218715 -0.7516 0.2440557 -0.2558969 -0.7516 0.2196846 -0.08665293 -0.7516 -0.2904722 0.1922612 -0.7516 -0.2904722 0.2632086 -0.7516 0.2196846 0.08665293 -0.7516 -0.5019719 0.1922612 -0.7516 -0.2991126 0.06803309 -0.7516 -0.302941 0 -0.7516 -0.2991126 -0.06803309 -0.7516 -0.5019719 -0.1922612 -0.7516 0.1556287 -0.04569667 -0.06152498 0.1621989 0 -0.6466 0.1621989 0 -0.06152498 0.1556287 0.04569667 -0.6466 0.1556287 0.04569667 -0.06152498 0.1364504 0.08769136 -0.6466 0.1364504 0.08769136 -0.06152498 0.1062177 0.1225817 -0.6466 0.1062177 0.1225817 -0.06152498 0.06737983 0.1475413 -0.6466 0.06737983 0.1475413 -0.06152498 0.02308326 0.160548 -0.6466 0.02308326 0.160548 -0.06152498 -0.02308326 0.160548 -0.6466 -0.02308326 0.160548 -0.06152498 -0.06737983 0.1475413 -0.6466 -0.06737983 0.1475413 -0.06152498 -0.1062177 0.1225817 -0.6466 -0.1062177 0.1225817 -0.06152498 -0.1364504 0.08769136 -0.6466 -0.1364504 0.08769136 -0.06152498 -0.1556287 0.04569667 -0.6466 -0.1556287 0.04569667 -0.06152498 -0.1621989 0 -0.6466 -0.1621989 0 -0.06152498 -0.1556287 -0.04569667 -0.6466 -0.1556287 -0.04569667 -0.06152498 -0.1364504 -0.08769136 -0.6466 -0.1364504 -0.08769136 -0.06152498 -0.1062177 -0.1225817 -0.6466 -0.1062177 -0.1225817 -0.06152498 -0.06737983 -0.1475413 -0.6466 -0.06737983 -0.1475413 -0.06152498 -0.02308326 -0.160548 -0.6466 -0.02308326 -0.160548 -0.06152498 0.02308326 -0.160548 -0.6466 0.02308326 -0.160548 -0.06152498 0.06737983 -0.1475413 -0.6466 0.06737983 -0.1475413 -0.06152498 0.1062177 -0.1225817 -0.6466 0.1062177 -0.1225817 -0.06152498 0.1364504 -0.08769136 -0.6466 0.1364504 -0.08769136 -0.06152498 0.1556287 -0.04569667 -0.6466 -0.1621989 0 -0.06152498 0.2196846 0.08665293 -0.6466 0.2196846 -0.08665293 -0.6466 -0.2991126 0.06803309 -0.6466 0 0.1922612 -0.6466 -0.2991126 -0.06803309 -0.6466 0 -0.1922612 -0.6466 -0.3381437 -0.2748523 -0.6466 -0.5019719 -0.1922612 -0.6466 -0.385532 -0.4156633 -0.6466 -0.5019719 -0.4156633 -0.6466 -0.302941 0 -0.6466 -0.385532 0.4156633 -0.6466 -0.5019719 0.4156633 -0.6466 -0.3381437 0.2748523 -0.6466 -0.5019719 0.1922612 -0.6466 0.2440557 -0.4156633 -0.6466 0.4170677 -0.4156633 -0.6466 0.2440557 -0.2558969 -0.6466 0.4170677 -0.218715 -0.6466 0.2440557 0.2558969 -0.6466 0.4170677 0.218715 -0.6466 0.2440557 0.4156633 -0.6466 0.4170677 0.4156633 -0.6466 -0.5019719 -0.1922612 -0.92418 -0.5019719 -0.4156633 -0.92418 -0.385532 -0.4156633 -0.92418 -0.3381437 -0.2748523 -0.92418 -0.2904722 -0.2632086 -0.92418 0.2440557 -0.2558969 -0.92418 0.2440557 -0.4156633 -0.92418 0.4170677 -0.4156633 -0.92418 0.4170677 -0.218715 -0.92418 0.4170677 0.218715 -0.92418 0.4170677 0.4156633 -0.92418 0.2440557 0.4156633 -0.92418 0.2440557 0.2558969 -0.92418 -0.2904722 0.2632086 -0.92418 -0.3381437 0.2748523 -0.92418 -0.385532 0.4156633 -0.92418 -0.5019719 0.4156633 -0.92418 -0.5019719 0.1922612 -0.92418 -0.2904722 0.1922612 -0.92418 -0.2904722 -0.1922612 -0.92418</float_array>
+          <technique_common>
+            <accessor source="#PEDESTAL-L-mesh-positions-array" count="105" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <source id="PEDESTAL-L-mesh-normals">
+          <float_array id="PEDESTAL-L-mesh-normals-array" count="612">0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0.9898214 -0.1423147 0 0.9898214 0.1423147 0 0.9898214 0.1423147 0 0.909632 0.4154151 0 0.909632 0.4154151 0 0.7557495 0.6548608 0 0.7557495 0.6548608 0 0.5406409 0.8412535 0 0.5406409 0.8412535 0 0.2817326 0.9594931 0 0.2817326 0.9594931 0 0 1 0 0 1 0 -0.2817326 0.9594931 0 -0.2817326 0.9594931 0 -0.5406409 0.8412535 0 -0.5406409 0.8412535 0 -0.7557495 0.6548608 0 -0.7557495 0.6548608 0 -0.909632 0.4154151 0 -0.909632 0.4154151 0 -0.9898214 0.1423147 0 -0.9898214 0.1423147 0 -0.9898214 -0.1423147 0 -0.9898214 -0.1423147 0 -0.909632 -0.4154151 0 -0.909632 -0.4154151 0 -0.7557495 -0.6548608 0 -0.7557495 -0.6548608 0 -0.5406409 -0.8412535 0 -0.5406409 -0.8412535 0 -0.2817326 -0.9594931 0 -0.2817326 -0.9594931 0 0 -1 0 0 -1 0 0.2817326 -0.9594931 0 0.2817326 -0.9594931 0 0.5406409 -0.8412535 0 0.5406409 -0.8412535 0 0.7557495 -0.6548608 0 0.7557495 -0.6548608 0 0.909632 -0.4154151 0 0.909632 -0.4154151 0 0.9898214 -0.1423147 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 -0.9984205 -0.05618375 0 -0.9984205 -0.05618375 0 -0.522241 0.8527979 0 -0.522241 0.8527979 0 -1 0 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 0.947768 -0.3189606 0 0.947768 -0.3189606 0 0.2372734 -0.9714429 0 0.2372734 -0.971443 0 0.2372732 -0.971443 0 0.2372732 -0.9714429 0 -0.252307 -0.9676472 0 -0.252307 -0.9676472 0 -1 0 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 1 0 0 1 0 0 1 0 0 0.5560793 0.8311293 0 0.5560793 0.8311293 0 1 0 0 1 0 0 0.5560793 -0.8311293 0 0.5560793 -0.8311293 0 1 0 0 1 0 0 1 0 0 0 1 0 0 1 0 -1 0 0 -1 0 0 -1 0 0 -0.252307 0.9676472 0 -0.252307 0.9676472 0 0.2372732 0.971443 0 0.2372732 0.9714429 0 0.2372734 0.9714429 0 0.2372734 0.971443 0 0.947768 0.3189606 0 0.947768 0.3189606 0 0 1 0 0 1 0 -1 0 0 -1 0 0 -1 0 0 -0.522241 -0.8527979 0 -0.522241 -0.8527979 0 -0.9984205 0.05618375 0 -0.9984205 0.05618375 0 1 0 0 1 0 0 0 -1 0 0 -1 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.2101123 -0.9776773 0 -0.2101123 -0.9776773 0 0 0 -1 0 0 -1 -0.2101123 0.9776773 0 -0.2101123 0.9776773 0 0 0 -1 0 0 -1 0 1 0 0 1 0 1 0 0 1 0 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1</float_array>
+          <technique_common>
+            <accessor source="#PEDESTAL-L-mesh-normals-array" count="204" stride="3">
+              <param name="X" type="float"/>
+              <param name="Y" type="float"/>
+              <param name="Z" type="float"/>
+            </accessor>
+          </technique_common>
+        </source>
+        <vertices id="PEDESTAL-L-mesh-vertices">
+          <input semantic="POSITION" source="#PEDESTAL-L-mesh-positions"/>
+        </vertices>
+        <polylist count="204">
+          <input semantic="VERTEX" source="#PEDESTAL-L-mesh-vertices" offset="0"/>
+          <input semantic="NORMAL" source="#PEDESTAL-L-mesh-normals" offset="1"/>
+          <vcount>3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 </vcount>
+          <p>0 0 1 0 2 0 3 1 4 1 5 1 6 2 7 2 8 2 8 3 7 3 9 3 9 4 10 4 8 4 8 5 10 5 3 5 8 6 3 6 11 6 11 7 3 7 5 7 12 8 9 8 13 8 13 9 9 9 7 9 13 10 7 10 14 10 14 11 7 11 0 11 14 12 0 12 15 12 15 13 0 13 2 13 15 14 2 14 16 14 17 15 18 15 19 15 19 16 18 16 20 16 19 17 20 17 21 17 21 18 20 18 22 18 21 19 22 19 23 19 23 20 22 20 24 20 23 21 24 21 25 21 25 22 24 22 26 22 25 23 26 23 27 23 27 24 26 24 28 24 27 25 28 25 29 25 29 26 28 26 30 26 29 27 30 27 31 27 31 28 30 28 32 28 31 29 32 29 33 29 33 30 32 30 34 30 33 31 34 31 35 31 35 32 34 32 36 32 35 33 36 33 37 33 37 34 36 34 38 34 37 35 38 35 39 35 39 36 38 36 40 36 39 37 40 37 41 37 41 38 40 38 42 38 41 39 42 39 43 39 43 40 42 40 44 40 43 41 44 41 45 41 45 42 44 42 46 42 45 43 46 43 47 43 47 44 46 44 48 44 47 45 48 45 49 45 49 46 48 46 50 46 49 47 50 47 51 47 51 48 50 48 52 48 51 49 52 49 53 49 53 50 52 50 54 50 53 51 54 51 55 51 55 52 54 52 56 52 55 53 56 53 57 53 57 54 56 54 58 54 57 55 58 55 59 55 59 56 58 56 60 56 59 57 60 57 17 57 17 58 60 58 18 58 27 59 29 59 47 59 39 60 61 60 43 60 29 61 31 61 47 61 47 62 31 62 33 62 47 63 33 63 45 63 27 64 47 64 25 64 25 65 47 65 49 65 25 66 49 66 51 66 51 67 53 67 25 67 25 68 53 68 55 68 25 69 55 69 57 69 39 70 43 70 37 70 45 71 33 71 43 71 43 72 33 72 35 72 43 73 35 73 37 73 17 74 19 74 21 74 57 75 59 75 25 75 25 76 59 76 17 76 25 77 17 77 23 77 23 78 17 78 21 78 20 79 18 79 62 79 58 80 56 80 63 80 64 81 36 81 34 81 32 82 30 82 65 82 65 83 30 83 28 83 48 84 46 84 66 84 54 85 52 85 67 85 67 86 52 86 50 86 50 87 48 87 67 87 67 88 48 88 66 88 67 89 66 89 68 89 68 90 66 90 69 90 68 91 69 91 70 91 70 92 69 92 71 92 46 93 44 93 66 93 66 94 44 94 42 94 66 95 42 95 72 95 72 96 42 96 40 96 72 97 40 97 64 97 64 98 40 98 38 98 64 99 38 99 36 99 73 100 74 100 75 100 75 101 74 101 76 101 75 102 76 102 65 102 65 103 76 103 64 103 65 104 64 104 32 104 32 105 64 105 34 105 62 106 18 106 63 106 63 107 18 107 60 107 63 108 60 108 58 108 77 109 78 109 79 109 79 110 78 110 80 110 79 111 80 111 67 111 67 112 80 112 63 112 67 113 63 113 54 113 54 114 63 114 56 114 26 115 24 115 62 115 62 116 24 116 22 116 62 117 22 117 20 117 28 118 26 118 65 118 65 119 26 119 62 119 65 120 62 120 81 120 81 121 62 121 82 121 81 122 82 122 83 122 83 123 82 123 84 123 66 124 72 124 15 124 15 125 72 125 14 125 69 126 66 126 16 126 16 127 66 127 15 127 85 128 86 128 16 128 16 129 86 129 71 129 16 130 71 130 69 130 87 131 70 131 86 131 86 132 70 132 71 132 88 133 68 133 87 133 87 134 68 134 70 134 0 135 67 135 1 135 1 136 67 136 68 136 1 137 68 137 89 137 89 138 68 138 88 138 79 139 67 139 7 139 7 140 67 140 0 140 90 141 91 141 7 141 7 142 91 142 77 142 7 143 77 143 79 143 92 144 78 144 91 144 91 145 78 145 77 145 80 146 78 146 6 146 6 147 78 147 92 147 6 148 92 148 93 148 63 149 80 149 8 149 8 150 80 150 6 150 62 151 63 151 11 151 11 152 63 152 8 152 82 153 62 153 5 153 5 154 62 154 11 154 94 155 95 155 5 155 5 156 95 156 84 156 5 157 84 157 82 157 96 158 83 158 95 158 95 159 83 159 84 159 96 160 97 160 83 160 83 161 97 161 4 161 83 162 4 162 81 162 65 163 81 163 3 163 3 164 81 164 4 164 98 165 99 165 10 165 10 166 99 166 75 166 10 167 75 167 3 167 3 168 75 168 65 168 100 169 73 169 99 169 99 170 73 170 75 170 101 171 74 171 100 171 100 172 74 172 73 172 76 173 74 173 12 173 12 174 74 174 101 174 12 175 101 175 102 175 64 176 76 176 13 176 13 177 76 177 12 177 72 178 64 178 14 178 14 179 64 179 13 179 10 180 9 180 98 180 98 181 9 181 103 181 9 182 12 182 103 182 103 183 12 183 102 183 98 184 103 184 99 184 99 185 103 185 102 185 99 186 102 186 100 186 100 187 102 187 101 187 5 188 4 188 94 188 94 189 4 189 97 189 96 190 95 190 97 190 97 191 95 191 94 191 7 192 6 192 90 192 90 193 6 193 93 193 92 194 91 194 93 194 93 195 91 195 90 195 16 196 2 196 85 196 85 197 2 197 104 197 2 198 1 198 104 198 104 199 1 199 89 199 87 200 86 200 88 200 88 201 86 201 85 201 88 202 85 202 89 202 89 203 85 203 104 203</p>
+        </polylist>
+      </mesh>
+      <extra><technique profile="MAYA"><double_sided>1</double_sided></technique></extra>
+    </geometry>
+  </library_geometries>
+  <library_controllers/>
+  <library_visual_scenes>
+    <visual_scene id="Scene" name="Scene">
+      <node id="PEDESTAL-L" name="PEDESTAL-L" type="NODE">
+        <matrix sid="transform">1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1</matrix>
+        <instance_geometry url="#PEDESTAL-L-mesh"/>
+      </node>
+    </visual_scene>
+  </library_visual_scenes>
+  <scene>
+    <instance_visual_scene url="#Scene"/>
+  </scene>
+</COLLADA>