kjyv / flobaroid Goto Github PK

View Code? Open in Web Editor NEW

61.0 6.0 25.0 50.69 MB

Framework for dynamical system identification of floating-base rigid body tree structures

License: GNU Lesser General Public License v3.0

Python 92.07% CMake 0.76% C 0.04% C++ 3.90% CSS 0.69% HTML 0.19% JavaScript 2.30% Shell 0.05%

identification urdf dynamics-models excitation yarp measurements robotics parameter-estimation

flobaroid's People

Contributors

Stargazers

Watchers

flobaroid's Issues

Estimating parameters for one joint

Would it be possible to estimate the inertial parameters for on joint alone instead of the entire system? (Say, by generating a trajectory which involves changes in only 1 joint angle). Also, how does the system differentiate between errors in joint position tracking in the system and the error in the inertial parameter?

URDF Issues

I followed the kuka_lw4.urdf file provided in the repo. After running the ./trajectory.py script (with optional world.urdf model), no feasible solutions were found at the end.
Also, I am interested on my own URDF file for our robotic model and I followed the skeleton format of kuka_lwr4.urdf provided, I get the following error:

mouhyemen@khan-msi:~/desktop/research/FloBaRoID$ ./trajectory.py --filename jim.trajectory.npz --config configs/testing.yaml --model model/krang_from_kuka_v2.urdf --world model/world_kuka.urdf 
loaded model model/krang_from_kuka_v2.urdf
# DOFs: 7
Joints: ['lwr_0_joint', 'lwr_1_joint', 'lwr_2_joint', 'lwr_3_joint', 'lwr_4_joint', 'lwr_5_joint', 'lwr_6_joint']
# regressor outputs: 7
# links: 8 (+ 0 fake)
{0: 'krang_shoulder', 1: 'lwr_1_link', 2: 'lwr_2_link', 3: 'lwr_3_link', 4: 'lwr_4_link', 5: 'lwr_5_link', 6: 'lwr_6_link', 7: 'lwr_7_link'}
# params: 80 (94 will be identified)
loaded random structural regressor from model/krang_from_kuka_v2.urdf.regressor.npz
broken
loaded model model/krang_from_kuka_v2.urdf
# DOFs: 7
Joints: ['lwr_0_joint', 'lwr_1_joint', 'lwr_2_joint', 'lwr_3_joint', 'lwr_4_joint', 'lwr_5_joint', 'lwr_6_joint']
# regressor outputs: 7
# links: 8 (+ 0 fake)
{0: 'krang_shoulder', 1: 'lwr_1_link', 2: 'lwr_2_link', 3: 'lwr_3_link', 4: 'lwr_4_link', 5: 'lwr_5_link', 6: 'lwr_6_link', 7: 'lwr_7_link'}
# params: 80 (94 will be identified)
loaded random structural regressor from model/krang_from_kuka_v2.urdf.regressor.npz
/home/mouhyemen/desktop/research/FloBaRoID/excitation/optimizer.py:200: RuntimeWarning: invalid value encountered in true_divide
  old_com = self.model.xStdModel[i*10+1:i*10+4] / self.model.xStdModel[i*10],
World links: ['ground_link']
Running global optimization with ALPSO
call #1/612
wf 0.5
a [[0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3]]
b [[0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3]]
q [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Traceback (most recent call last):
  File "./trajectory.py", line 94, in <module>
    main()
  File "./trajectory.py", line 71, in main
    trajectory = trajectoryOptimizer.optimizeTrajectory()
  File "/home/mouhyemen/desktop/research/FloBaRoID/excitation/trajectoryOptimizer.py", line 390, in optimizeTrajectory
    sol_vec = self.runOptimizer(opt_prob)
  File "/home/mouhyemen/desktop/research/FloBaRoID/excitation/optimizer.py", line 504, in runOptimizer
    opt(opt_prob, store_hst=False) #, xstart=initial)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyOpt_optimizer.py", line 146, in __call__
    return self.__solve__(opt_problem, *args, **kwargs)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyALPSO/pyALPSO.py", line 442, in __solve__
    hos_file, seed, scale, nhs, objconfunc)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyALPSO/alpso.py", line 253, in alpso
    [f[i], g[i, :]] = objfunc(xtmp)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyALPSO/pyALPSO.py", line 293, in objconfunc
    [ff, gg, fail] = opt_problem.obj_fun(xn, *args, **kwargs)
  File "/home/mouhyemen/desktop/research/FloBaRoID/excitation/trajectoryOptimizer.py", line 257, in objectiveFunc
    if d < g[c_s + g_cnt]:
IndexError: list index out of range

Note: If I do NOT include the world.urdf model, the above error is generated even for kuka_lw4r.urdf.

Extending the identification for mobile robots

Hey,

Is it possible to use your tool for a holonomic mobile robot?

Trajectory file not generated

I installed all dependencies and individually tested if Python can import the libraries.

For the trajectory script (others as well), the trajectory file is not being generated. I am also getting warnings such as:
[WARNING] The iDynTree::Regressors::DynamicsRegressorGenerator class still needs to be migrated to
[WARNING] iDynTree native classes. To continue to use the iDynTree::HighLevel::DynamicsComputations class in the
[WARNING] bindings in the current form, please compile iDynTree with the IDYNTREE_USES_KDL CMake option.
[WARNING] The iDynTree::Regressors::DynamicsRegressorGenerator class still needs to be migrated to
[WARNING] iDynTree native classes. To continue to use the iDynTree::HighLevel::DynamicsComputations class in the
[WARNING] bindings in the current form, please compile iDynTree with the IDYNTREE_USES_KDL CMake option

Is the script dependent on this warning and that's why none of the trajectories are generated?? How did you solve this issue?

Identification & Excitation Issues

I am using the measurements.regressor.npz file generated from excite.py script into the identify.py script and this is the error I am getting with identify.py

mouhyemen@khan-msi:~/desktop/research/FloBaRoID$ ./identify.py --config configs/testing.yaml --model model/kuka_lwr4.urdf --measurements test_new_measurement.npz --output model/example_identified.urdf
loaded model model/kuka_lwr4.urdf
# DOFs: 7
Joints: ['lwr_0_joint', 'lwr_1_joint', 'lwr_2_joint', 'lwr_3_joint', 'lwr_4_joint', 'lwr_5_joint', 'lwr_6_joint']
# regressor outputs: 7
# links: 8 (+ 0 fake)
{0: 'lwr_base_link', 1: 'lwr_1_link', 2: 'lwr_2_link', 3: 'lwr_3_link', 4: 'lwr_4_link', 5: 'lwr_5_link', 6: 'lwr_6_link', 7: 'lwr_7_link'}
# params: 80 (94 will be identified)
loaded random structural regressor from model/kuka_lwr4.urdf.regressor.npz
loaded 709 measurement samples (using 709)
computing standard regressor matrix for data samples
100%|███████████████████████████████████████████████████████████████████████████████| 709/709 [00:00<00:00, 3018.16it/s]
estimating parameters using regressor
./identify.py:590: FutureWarning: `rcond` parameter will change to the default of machine precision times ``max(M, N)`` where M and N are the input matrix dimensions.
To use the future default and silence this warning we advise to pass `rcond=None`, to keep using the old, explicitly pass `rcond=-1`.
  self.model.xBase = la.lstsq(YBase, tau)[0]
/usr/local/lib/python2.7/dist-packages/numpy/linalg/linalg.py:2054: RuntimeWarning: overflow encountered in square
  resids = sum(r_parts**2, axis=-2)
/usr/local/lib/python2.7/dist-packages/numpy/linalg/linalg.py:2286: RuntimeWarning: overflow encountered in multiply
  s = (x.conj() * x).real
Initializing LMIs...
Traceback (most recent call last):
  File "./identify.py", line 1096, in <module>
    main()
  File "./identify.py", line 1074, in main
    idf.estimateParameters()
  File "./identify.py", line 762, in estimateParameters
    self.sdp.initSDP_LMIs(self)
  File "/home/mouhyemen/desktop/research/FloBaRoID/identification/sdp.py", line 306, in initSDP_LMIs
    self.LMIs = list(map(LMI_PD, self.D_blocks))
  File "/home/mouhyemen/desktop/research/FloBaRoID/identification/sdp_helpers.py", line 28, in LMI_PD
    lmi = lhs > sympify(rhs)
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/numbers.py", line 2133, in __lt__
    return Rational.__lt__(self, other)
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/numbers.py", line 1791, in __lt__
    return Expr.__lt__(expr, other)
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/expr.py", line 328, in __lt__
    dif = self - other
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/numbers.py", line 2057, in __sub__
    return Rational.__sub__(self, other)
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/decorators.py", line 91, in __sympifyit_wrapper
    return func(a, b)
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/numbers.py", line 1577, in __sub__
    return Number.__sub__(self, other)
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/decorators.py", line 91, in __sympifyit_wrapper
    return func(a, b)
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/numbers.py", line 652, in __sub__
    return AtomicExpr.__sub__(self, other)
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/decorators.py", line 91, in __sympifyit_wrapper
    return func(a, b)
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/decorators.py", line 131, in binary_op_wrapper
    return f(self)
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/decorators.py", line 132, in binary_op_wrapper
    return func(self, other)
  File "/usr/local/lib/python2.7/dist-packages/sympy/matrices/common.py", line 2092, in __rsub__
    return (-self) + a
  File "/usr/local/lib/python2.7/dist-packages/sympy/core/decorators.py", line 132, in binary_op_wrapper
    return func(self, other)
  File "/usr/local/lib/python2.7/dist-packages/sympy/matrices/common.py", line 1964, in __add__
    raise TypeError('cannot add %s and %s' % (type(self), type(other)))
TypeError: cannot add <class 'sympy.matrices.immutable.ImmutableDenseMatrix'> and <class 'sympy.core.numbers.Zero'>

Also, the excite.py script does not work with the regressor.npz files generated from our custom-made URDF files. The custom URDF file I am using is also 7DOF like Kuka's with only mass, com, axes of rotations, and inertial values changed.

Issue with the static trajectory generation

As the trajectory generation does not provide a feasible solution , I tried out the static trajectory generation I am getting following error can you help me out .

SDP found std solution with 0.0968628270356 squared residual error
Objective function value: 0.000390542557771 (last best: inf)
(Gradient evaluation)
Parameter error: [ 8.22906798e-03 1.11528984e-04 2.00252935e-04 -1.97377992e-05 2.64326747e-03 -8.39580773e-03 2.14005092e-03 -1.73155946e-03 -6.80186293e-05
7.80794591e-04 -1.21921512e-02 9.40503509e-03]
Constraints not met.

and finally there is no feasible solution again

python Excution problem with idyntree

Version: ROS noetic

I compile the idyntree with the guidance in https://github.com/robotology/robotology-superbuild , with python-binding .
However, when I run:
./trajectory.py ....
I get the error:

Traceback (most recent call last):
  File "./trajectory.py", line 10, in <module>
    import iDynTree; iDynTree.init_helpers(); iDynTree.init_numpy_helpers()
ModuleNotFoundError: No module named 'iDynTree'

Then,I doubt that the module was not exported in the $PYTHONPATH, so I turn on a terminal and run python3 to verify the pythonpath.
However, iDynTree was still not found while the module with the name "idyntree" was found. How could this happen? Is there something wrong with the installation of iDynTree? Why the module name could be different?

I would appreciate any help from you!

ImportError: cannot import name transform (fcl)

Hi, I'm trying to identify the dynamic parameters of the ABB irb 6640 robot for my research internship with Flobaroid. when I execute the trajectory.py script to generate an optimal exciting trajectory, an import error block the execution of the script.

from fcl import fcl, collision_data, transform
ImportError: cannot import name transform

I've searched in the library fcl but I can't find the transform module.
I'm using :

Ubuntu 18.04 with Ros melodic
python 2.7 with PyCharm

geekayman@geekayman-Lenovo-ideapad-330-15ICH:~/robot_iden/FloBaRoID$ ./trajectory.py --config configs/kuka_lwr4.yaml --model model/kuka_lwr4.urdf --world model/world_kuka.urdf
Error: IPOPT shared library failed to import
./trajectory.py:30: YAMLLoadWarning: calling yaml.load() without Loader=... is deprecated, as the default Loader is unsafe. Please read https://msg.pyyaml.org/load for full details.
  config = yaml.load(stream)
loaded model model/kuka_lwr4.urdf
# DOFs: 7
Joints: ['lwr_0_joint', 'lwr_1_joint', 'lwr_2_joint', 'lwr_3_joint', 'lwr_4_joint', 'lwr_5_joint', 'lwr_6_joint']
# regressor outputs: 7
# links: 8 (+ 0 fake)
{0: 'lwr_base_link', 1: 'lwr_1_link', 2: 'lwr_2_link', 3: 'lwr_3_link', 4: 'lwr_4_link', 5: 'lwr_5_link', 6: 'lwr_6_link', 7: 'lwr_7_link'}
# params: 80 (94 will be identified)
loaded random structural regressor from model/kuka_lwr4.urdf.regressor.npz
loaded model model/kuka_lwr4.urdf
# DOFs: 7
Joints: ['lwr_0_joint', 'lwr_1_joint', 'lwr_2_joint', 'lwr_3_joint', 'lwr_4_joint', 'lwr_5_joint', 'lwr_6_joint']
# regressor outputs: 7
# links: 8 (+ 0 fake)
{0: 'lwr_base_link', 1: 'lwr_1_link', 2: 'lwr_2_link', 3: 'lwr_3_link', 4: 'lwr_4_link', 5: 'lwr_5_link', 6: 'lwr_6_link', 7: 'lwr_7_link'}
# params: 80 (94 will be identified)
loaded random structural regressor from model/kuka_lwr4.urdf.regressor.npz
World links: ['ground_link']
Running global optimization with ALPSO
call #1/612
wf 0.5
a [[0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3]]
b [[0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3], [0.3, 0.3, 0.3, 0.3]]
q [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Traceback (most recent call last):
  File "./trajectory.py", line 91, in <module>
    main()
  File "./trajectory.py", line 68, in main
    trajectory = trajectoryOptimizer.optimizeTrajectory()
  File "/home/geekayman/robot_iden/FloBaRoID/excitation/trajectoryOptimizer.py", line 390, in optimizeTrajectory
    sol_vec = self.runOptimizer(opt_prob)
  File "/home/geekayman/robot_iden/FloBaRoID/excitation/optimizer.py", line 504, in runOptimizer
    opt(opt_prob, store_hst=False) #, xstart=initial)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyOpt_optimizer.py", line 146, in __call__
    return self.__solve__(opt_problem, *args, **kwargs)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyALPSO/pyALPSO.py", line 442, in __solve__
    hos_file, seed, scale, nhs, objconfunc)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyALPSO/alpso.py", line 253, in alpso
    [f[i], g[i, :]] = objfunc(xtmp)
  File "/usr/local/lib/python2.7/dist-packages/pyOpt/pyALPSO/pyALPSO.py", line 293, in objconfunc
    [ff, gg, fail] = opt_problem.obj_fun(xn, *args, **kwargs)
  File "/home/geekayman/robot_iden/FloBaRoID/excitation/trajectoryOptimizer.py", line 256, in objectiveFunc
    d = self.getLinkDistance(l0_name, l1_name, q)
  File "/home/geekayman/robot_iden/FloBaRoID/excitation/optimizer.py", line 245, in getLinkDistance
    from fcl import fcl, collision_data, transform
ImportError: cannot import name transform

pyOpt TypeError: 'bool' object is not iterable

@kjyv ,could you help me?When i run parallel_optimize_kuka_lwr4.sh,i got the following error.I'm almost get the solution,because it have runed 250 iterations(153 iterations for global optimizer,the rest for local).What's more, i run optimize_kuka_lwr4.sh could get well solution,not error.
I use ubuntu 16.06 and python2.7,pyOpt from your readme.

capi_return is NULL
Call-back cb_slgrad_in_slsqp__user__routines failed.
Traceback (most recent call last):
File "./trajectory.py", line 91, in
main()
File "./trajectory.py", line 68, in main
trajectory = trajectoryOptimizer.optimizeTrajectory()
File "/home/kiwi/soft/learn/IDIM/FloBaRoID/excitation/trajectoryOptimizer.py", line 394, in optimizeTrajectory
sol_vec = self.runOptimizer(opt_prob)
File "/home/kiwi/soft/learn/IDIM/FloBaRoID/excitation/optimizer.py", line 573, in runOptimizer
opt2(opt_prob, sens_step=0.1, sens_mode='pgc', store_hst=False)
File "/home/kiwi/anaconda2/envs/FloBaRoID/lib/python2.7/site-packages/pyOpt/pyOpt_optimizer.py", line 146, in call
return self.solve(opt_problem, *args, **kwargs)
File "/home/kiwi/anaconda2/envs/FloBaRoID/lib/python2.7/site-packages/pyOpt/pySLSQP/pySLSQP.py", line 398, in solve
slfunc, slgrad)
File "/home/kiwi/anaconda2/envs/FloBaRoID/lib/python2.7/site-packages/pyOpt/pySLSQP/pySLSQP.py", line 292, in slgrad
kwargs)
File "/home/kiwi/anaconda2/envs/FloBaRoID/lib/python2.7/site-packages/pyOpt/pyOpt_gradient.py", line 288, in getGrad
[df, dg] = self.Bcast([df, dg], root=0)
TypeError: 'bool' object is not iterable

Investigate alternatives for inequality constraint fitting

Investigate using for alternative inequality constraint fitting: https://lmfit.github.io/lmfit-py/examples/example_fit_with_inequality.html#sphx-glr-examples-example-fit-with-inequality-py

Does FloBaRoID platform allow frictional (coulomb/viscous) parameters?

I am finally able to run the trajectory script. Thanks on that end!

I know that URDF provides a way to handle frictional coefficients.

  <!-- Second segment of the arm. Joint along z-axis -->
  <joint name="lwr_0_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.11"/>
    <axis xyz="0 0 1"/>
    <limit effort="176" lower="-2.96705972839" upper="2.96705972839" velocity="1.91986217719"/>
    <dynamics damping="1.0" friction="0.0"/>
    <parent link="lwr_base_link"/>
    <child link="lwr_1_link"/>
  </joint>

Does FloBaRoID account for friction while computing optimal trajectories? Specifically, to handle rotor inertias, coulomb/viscous frictions? If not the URDF, are there other config files where we can tweak these values?

Installed ROS | rospkg.common.ResourceNotFound: lwr_description

I am not sure what the error could be here. Is FloBaRoID heavily dependent on ROS packages?
I am getting an error around lwr_description and I am not sure how to work around this.

This is my output after running /trajectory.py (command highlighted in yellow, error in red):

No file "dsdp5.out"

outfile = open(os.path.join(dir, 'dsdp5.out'), 'r').readlines()
IOError: [Errno 2] No such file or directory: '/home/kingsfu/DynaCalib/FloBaRoID/identification/sdpa_dat_9c4504c6-a4b0-44b4-8294-cc07f41ec7bc/dsdp5.out'

kjyv / flobaroid Goto Github PK

flobaroid's People

Contributors

Stargazers

Watchers

Forkers

flobaroid's Issues

Recommend Projects

Recommend Topics

Recommend Org