-
Notifications
You must be signed in to change notification settings - Fork 65
/
KinDynComputationsTutorial.py
63 lines (47 loc) · 1.95 KB
/
KinDynComputationsTutorial.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
# -*- coding: utf-8 -*-
"""
To run this example, first install iDynTree for python,
then navigate to this directory and run:
python KinDynComputationsTutorial.py
Otherwise, modify the URDF_FILE parameter to point to an existing
URDF in your system.
"""
import idyntree.bindings as iDynTree
URDF_FILE = '../../src/tests/data/icub.urdf';
dynComp = iDynTree.KinDynComputations();
mdlLoader = iDynTree.ModelLoader();
mdlLoader.loadModelFromFile(URDF_FILE);
dynComp.loadRobotModel(mdlLoader.model());
print("The loaded model has", dynComp.model().getNrOfDOFs(), \
"internal degrees of freedom and",dynComp.model().getNrOfLinks(),"links.")
dofs = dynComp.model().getNrOfDOFs();
# For the sake of the example, we fill the joints vector with gibberish data (remember in any case
# that all quantities are expressed in radians-based units
s = [1.0]*dofs
ds = [0.4]*dofs
dds = [0.3]*dofs
# The gravity acceleration is a 3d acceleration vector.
g = [0.0, 0.0, -9.81]
dynComp.setRobotState(s, ds, g)
jac = iDynTree.MatrixDynSize(6,6+dofs);
ok = dynComp.getFrameFreeFloatingJacobian("l_sole", jac);
if( not ok ):
print("Error in computing jacobian of frame " + "l_sole");
else:
print("Jacobian of lf_foot is\n" + jac.toString());
root_H_sole = dynComp.getRelativeTransform("root_link", "l_sole");
root_R_sole = root_H_sole.getRotation()
if( not ok ):
print("Error in computing jacobian of frame " + "l_sole");
else:
print("Orientation between root_link and l_sole is\n" + str(root_R_sole.toNumPy()));
print("In RPY format:\n" + str(root_R_sole.asRPY().toNumPy()))
baseAcc = [0.0]*6
links = dynComp.model().getNrOfLinks();
regr = iDynTree.MatrixDynSize(6+dofs,10*links);
ok = dynComp.inverseDynamicsInertialParametersRegressor(baseAcc, dds, regr);
# The Dynamics Regressor is quite big, uncomment if you really want to print it
#if( not ok ):
# print("Error in computing the dynamics regressor");
#else :
# print("The dynamics regressor is\n" + regr.toString());