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;
'''
For testing iDynTree python bindings, we rely on the unittest standard python lib
'''
import os
# This test is mean to be executed from the build,
# so we add in PYTHONPATH the location of iDynTree.py and _iDynTree.so
os.environ["PYTHONPATH"] = os.environ["PYTHONPATH"] + ":../:../../../lib/python/"
import unittest
import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers()
import numpy as np
import random
class DynCompTest(unittest.TestCase):
'''Helper methods'''
def checkApproxEqual(self,val1,val2,msg):
self.assertAlmostEqual(val1,val2,self.places(),msg)
def checkSpatialVectorEqual(self,p1,p2,msg):
msgMore = "val1 = " + p1.toString() + " and val2 = " + p2.toString()
for i in range(0,3):
self.checkApproxEqual(p1.getVal(i),p2.getVal(i),msg+":"+msgMore)
def checkVectorEqual(self,v1,v2,msg):
msgMore = "val1 = " + v1.toString() + " and val2 = " + v2.toString()
for i in range(0,v1.size()):
self.checkApproxEqual(v1.getVal(i), v2.getVal(i), msg+":"+msgMore)
'''tests'''
def testDynComp(self):
#!/usr/bin/env python
from builtins import range
import numpy as np
import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers()
import identificationHelpers
import argparse
def main():
'''
open a urdf file and add noise to each parameter. Can be used for testing, but noisy params are usually not consistent, so may be of limited use.
'''
parser = argparse.ArgumentParser(description='Load measurements and URDF model to get inertial parameters.')
parser.add_argument('--urdf_input', required=True, type=str, help='the file to load the robot model from')
parser.add_argument('--urdf_output', required=True, type=str, help='the file to save the noisy robot model to')
parser.add_argument('--noise', required=False, type=float, help='scale of noise (default 0.01)')
parser.set_defaults(noise=0.01)
args = parser.parse_args()
model = iDynTree.Model()
iDynTree.modelFromURDF(args.urdf_input, model)
link_names = []
for i in range(0, model.getNrOfLinks()):
link_names.append(model.getLinkName(i))
n_params = model.getNrOfLinks()*10
dynComp = iDynTree.DynamicsComputations()
dynComp.loadRobotModelFromFile(args.urdf_input)
xStdModel = iDynTree.VectorDynSize(n_params)
dynComp.getModelDynamicsParameters(xStdModel)
xStdModel = xStdModel.toNumPy()
#!/usr/bin/env python
#-*- coding: utf-8 -*-
from __future__ import division
from __future__ import print_function
from builtins import range
import sys
from typing import Dict
import numpy as np
from colorama import Fore, Back, Style
import iDynTree
iDynTree.init_helpers()
iDynTree.init_numpy_helpers()
import argparse
parser = argparse.ArgumentParser(
description=
'Send an excitation trajectory and record measurements to <filename>.')
parser.add_argument('--model',
required=True,
type=str,
help='the file to load the robot model from')
parser.add_argument('--filename',
type=str,
help='the filename to save the measurements to')
parser.add_argument('--trajectory',
type=str,
help='the file to load the trajectory from')
parser.add_argument('--config',
required=True,