7 from .
import pinocchio_pywrap_default
as pin
8 from .
import WITH_HPP_FCL, WITH_HPP_FCL_BINDINGS
9 from typing
import Tuple
11 nle = pin.nonLinearEffects
20 geometry_types=[pin.GeometryType.COLLISION, pin.GeometryType.VISUAL],
21 ) -> Tuple[pin.Model, pin.GeometryModel, pin.GeometryModel]:
22 """Parse the URDF file given in input and return a Pinocchio Model followed by corresponding GeometryModels of types specified by geometry_types, in the same order as listed.
24 # load model, collision model, and visual model, in this order (default)
25 model, collision_model, visual_model = buildModelsFromUrdf(filename[, ...], geometry_types=[pin.GeometryType.COLLISION,pin.GeometryType.VISUAL])
26 model, collision_model, visual_model = buildModelsFromUrdf(filename[, ...]) # same as above
28 model, collision_model = buildModelsFromUrdf(filename[, ...], geometry_types=[pin.GeometryType.COLLISION]) # only load the model and the collision model
29 model, collision_model = buildModelsFromUrdf(filename[, ...], geometry_types=pin.GeometryType.COLLISION) # same as above
30 model, visual_model = buildModelsFromUrdf(filename[, ...], geometry_types=pin.GeometryType.VISUAL) # only load the model and the visual model
32 model = buildModelsFromUrdf(filename[, ...], geometry_types=[]) # equivalent to buildModelFromUrdf(filename[, root_joint])
35 Remark: In the URDF format, a joint of type fixed can be defined.
36 For efficiency reasons, it is treated as operational frame and not as a joint of the model.
38 if geometry_types
is None:
39 geometry_types = [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL]
40 if root_joint
is None:
41 model = pin.buildModelFromUrdf(filename)
43 model = pin.buildModelFromUrdf(filename, root_joint)
45 if verbose
and not WITH_HPP_FCL
and meshLoader
is not None:
47 "Info: MeshLoader is ignored. Pinocchio has not been compiled with HPP-FCL."
49 if verbose
and not WITH_HPP_FCL_BINDINGS
and meshLoader
is not None:
51 "Info: MeshLoader is ignored. The HPP-FCL Python bindings have not been installed."
53 if package_dirs
is None:
58 if not hasattr(geometry_types,
"__iter__"):
59 geometry_types = [geometry_types]
61 for geometry_type
in geometry_types:
62 if meshLoader
is None or (
not WITH_HPP_FCL
and not WITH_HPP_FCL_BINDINGS):
63 geom_model = pin.buildGeomFromUrdf(
64 model, filename, geometry_type, package_dirs=package_dirs
67 geom_model = pin.buildGeomFromUrdf(
71 package_dirs=package_dirs,
72 mesh_loader=meshLoader,
74 lst.append(geom_model)
80 """Call createData() on each Model or GeometryModel in input and return the results in a tuple.
81 If one of the models is None, the corresponding data object in the result is also None.
83 return tuple([
None if model
is None else model.createData()
for model
in models])
96 """Parse the SDF file given in input and return a Pinocchio Model and a list of Constraint Models, followed by corresponding GeometryModels of types specified by geometry_types, in the same order as listed.
98 # load model, constraint models, collision model, and visual model, in this order (default)
99 model, constraint_models, collision_model, visual_model = buildModelsFromSdf(filename[, ...], geometry_types=[pin.GeometryType.COLLISION,pin.GeometryType.VISUAL])
100 model, constraint_models, collision_model, visual_model = buildModelsFromSdf(filename[, ...]) # same as above
101 model, constraint_models, collision_model = buildModelsFromSdf(filename[, ...], geometry_types=[pin.GeometryType.COLLISION]) # only load the model, constraint models and the collision model
102 model, constraint_models, collision_model = buildModelsFromSdf(filename[, ...], geometry_types=pin.GeometryType.COLLISION) # same as above
103 model, constraint_models, visual_model = buildModelsFromSdf(filename[, ...], geometry_types=pin.GeometryType.VISUAL) # only load the model and the visual model
104 model, constraint_models = buildModelsFromSdf(filename[, ...], geometry_types=[]) # equivalent to buildModelFromSdf(filename[, root_joint])
106 if geometry_types
is None:
107 geometry_types = [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL]
108 if root_joint
is None:
109 model, constraint_models = pin.buildModelFromSdf(
110 filename, root_link_name, parent_guidance
113 model, constraint_models = pin.buildModelFromSdf(
114 filename, root_joint, root_link_name, parent_guidance
117 if verbose
and not WITH_HPP_FCL
and meshLoader
is not None:
119 "Info: MeshLoader is ignored. Pinocchio has not been compiled with HPP-FCL."
121 if verbose
and not WITH_HPP_FCL_BINDINGS
and meshLoader
is not None:
123 "Info: MeshLoader is ignored. The HPP-FCL Python bindings have not been installed."
125 if package_dirs
is None:
128 lst = [model, constraint_models]
130 if not hasattr(geometry_types,
"__iter__"):
131 geometry_types = [geometry_types]
133 for geometry_type
in geometry_types:
134 if meshLoader
is None or (
not WITH_HPP_FCL
and not WITH_HPP_FCL_BINDINGS):
135 geom_model = pin.buildGeomFromSdf(
136 model, filename, geometry_type, root_link_name, package_dirs
139 geom_model = pin.buildGeomFromSdf(
140 model, filename, geometry_type, root_link_name, package_dirs, meshLoader
142 lst.append(geom_model)
148 filename, root_joint=None, verbose=False, meshLoader=None, geometry_types=None
150 """Parse the Mjcf file given in input and return a Pinocchio Model, followed by corresponding GeometryModels of types specified by geometry_types, in the same order as listed.
152 # load model, constraint models, collision model, and visual model, in this order (default)
153 # load model, collision model, and visual model, in this order (default)
154 model, collision_model, visual_model = buildModelsFromMJCF(filename[, ...], geometry_types=[pin.GeometryType.COLLISION,pin.GeometryType.VISUAL])
155 model, collision_model, visual_model = buildModelsFromMJCF(filename[, ...]) # same as above
157 model, collision_model = buildModelsFromMJCF(filename[, ...], geometry_types=[pin.GeometryType.COLLISION]) # only load the model and the collision model
158 model, collision_model = buildModelsFromMJCF(filename[, ...], geometry_types=pin.GeometryType.COLLISION) # same as above
159 model, visual_model = buildModelsFromMJCF(filename[, ...], geometry_types=pin.GeometryType.VISUAL) # only load the model and the visual model
161 model = buildModelsFromMJCF(filename[, ...], geometry_types=[]) # equivalent to buildModelFromMJCF(filename[, root_joint])
163 if geometry_types
is None:
164 geometry_types = [pin.GeometryType.COLLISION, pin.GeometryType.VISUAL]
165 if root_joint
is None:
166 model = pin.buildModelFromMJCF(filename)
168 model = pin.buildModelFromMJCF(filename, root_joint)
170 if verbose
and not WITH_HPP_FCL
and meshLoader
is not None:
172 "Info: MeshLoader is ignored. Pinocchio has not been compiled with HPP-FCL."
174 if verbose
and not WITH_HPP_FCL_BINDINGS
and meshLoader
is not None:
176 "Info: MeshLoader is ignored. The HPP-FCL Python bindings have not been installed."
181 if not hasattr(geometry_types,
"__iter__"):
182 geometry_types = [geometry_types]
184 for geometry_type
in geometry_types:
185 if meshLoader
is None or (
not WITH_HPP_FCL
and not WITH_HPP_FCL_BINDINGS):
186 geom_model = pin.buildGeomFromMJCF(model, filename, geometry_type)
188 geom_model = pin.buildGeomFromMJCF(
189 model, filename, geometry_type, mesh_loader=meshLoader
191 lst.append(geom_model)