def get_triangle_centers_from_urdf(urdfilename, linkName, skinFrameName,
trianglesNumbersList):
iDynTree.init_numpy_helpers()
mdlLoader = iDynTree.ModelLoader()
mdlLoader.loadModelFromFile(urdfilename)
kinDyn = iDynTree.KinDynComputations()
kinDyn.loadRobotModel(mdlLoader.model())
# The results is returned by this function already in the frame used by the skin
centersAndNormals = {}
centersAndNormals['centers'] = {}
centersAndNormals['normals'] = {}
for triangleNumber in trianglesNumbersList:
# Get center in link frame
triangleFrameName = linkName + "_skin_" + str(triangleNumber)
skinFrame_H_triangleFrameName = kinDyn.getRelativeTransform(
skinFrameName, triangleFrameName)
triangleCenter_wrt_skinFrame = skinFrame_H_triangleFrameName.getPosition(
)
skinFrame_R_triangleFrame = skinFrame_H_triangleFrameName.getRotation(
).toNumPy()
centersAndNormals['centers'][
triangleNumber] = triangleCenter_wrt_skinFrame.toNumPy()
centersAndNormals['normals'][
triangleNumber] = skinFrame_R_triangleFrame[:, 2]
return centersAndNormals
def get_model_loader(model_file: str, considered_joints: List[str] = None):
# Find the urdf file
urdf_file = resource_finder.find_resource(file_name=model_file)
# Get the file extension
folder, model_file = os.path.split(urdf_file)
model_name, extension = os.path.splitext(model_file)
if extension == ".sdf":
raise RuntimeError(
"SDF models are not currently supported by iDynTree")
# Create the model loader
mdl_loader = idt.ModelLoader()
# Load the urdf model
if considered_joints is None:
ok_load = mdl_loader.loadModelFromFile(urdf_file)
else:
ok_load = mdl_loader.loadReducedModelFromFile(
urdf_file, considered_joints)
if not ok_load:
raise RuntimeError(f"Failed to load model from file '{urdf_file}'")
return mdl_loader
def _get_model_loader(
urdf: str, joint_serialization: List[str] = None
) -> idt.ModelLoader:
# Get the model loader
model_loader = idt.ModelLoader()
if not os.path.exists(urdf):
raise FileNotFoundError(urdf)
# Load the model
if joint_serialization:
ok_load = model_loader.loadReducedModelFromFile(urdf, joint_serialization)
else:
ok_load = model_loader.loadModelFromFile(urdf)
if not ok_load:
raise RuntimeError("Failed to load model")
# Due to some SWIG internal, returning the model contained by the loader
# does not work as expected
return model_loader