Forward dynamics in python binding

[albindgit]

Is the python binding for computeH() and computeC() (alternatively the get functions H() and C()) tested correctly? When I try to use it I get nonsense while using the same robot model in c++ seems to give the correct matrices. Escpecially in the code below the same (incorrect) matrices are printed both before and after computeH() is called. The urdf model seems to have been parsed correctly with joints, bodies etc correct. Am I doing something wrong?

self.rbd = rbdyn.parsers.from_urdf(urdf_model) self.rbd.mbc.zero(self.rbd.mb) fd = rbdyn.ForwardDynamics(self.rbd.mb) rbdyn.forwardKinematics(self.rbd.mb, self.rbd.mbc) rbdyn.forwardVelocity(self.rbd.mb, self.rbd.mbc) a = np.squeeze(fd.H()) print(a) fd.computeH(self.rbd.mb, self.rbd.mbc) a = np.squeeze(fd.H()) print(a)

[gergondet]

Hi @albindgit

I can't reproduce the issue you're facing. I've tried the following C++ program:

#include <RBDyn/parsers/urdf.h>
#include <RBDyn/FD.h>
#include <RBDyn/FK.h>
#include <RBDyn/FV.h>

#include <iostream>

int main()
{
  auto rbd = rbd::parsers::from_urdf_file("panda_default.urdf");
  rbd.mbc.zero(rbd.mb);
  rbd::ForwardDynamics fd{rbd.mb};
  rbd::forwardKinematics(rbd.mb, rbd.mbc);
  rbd::forwardVelocity(rbd.mb, rbd.mbc);
  fd.computeH(rbd.mb, rbd.mbc);
  std::cout << fd.H() << "\n";
  return 0;
}

and the following Python program:

#!/usr/bin/env python3

import rbdyn
import numpy as np

rbd = rbdyn.parsers.from_urdf_file('panda_default.urdf')
rbd.mbc.zero(rbd.mb)
fd = rbdyn.ForwardDynamics(rbd.mb)
rbdyn.forwardKinematics(rbd.mb, rbd.mbc)
rbdyn.forwardVelocity(rbd.mb, rbd.mbc)
fd.computeH(rbd.mb, rbd.mbc)
print(fd.H())

And they produce the same output:

❯ ./test.py
   0.0787044   -0.0929847    0.0605446    0.0462454    0.0289808  0.000452923  -0.00136688
  -0.0929847      2.55747   -0.0916544      -1.0663   -0.0922459  -0.00350884   -0.0036085
   0.0605446   -0.0916544    0.0605446    0.0462454    0.0289808  0.000452923  -0.00136688
   0.0462454      -1.0663    0.0462454     0.580729    0.0464437 -0.000790874   0.00176618
   0.0289808   -0.0922459    0.0289808    0.0464437    0.0289808  0.000452923  -0.00136688
 0.000452923  -0.00350884  0.000452923 -0.000790874  0.000452923    0.0182122 -0.000472579
 -0.00136688   -0.0036085  -0.00136688   0.00176618  -0.00136688 -0.000472579  0.000845606
❯ ./test
   0.0787044   -0.0929847    0.0605446    0.0462454    0.0289808  0.000452923  -0.00136688
  -0.0929847      2.55747   -0.0916544      -1.0663   -0.0922459  -0.00350884   -0.0036085
   0.0605446   -0.0916544    0.0605446    0.0462454    0.0289808  0.000452923  -0.00136688
   0.0462454      -1.0663    0.0462454     0.580729    0.0464437 -0.000790874   0.00176618
   0.0289808   -0.0922459    0.0289808    0.0464437    0.0289808  0.000452923  -0.00136688
 0.000452923  -0.00350884  0.000452923 -0.000790874  0.000452923    0.0182122 -0.000472579
 -0.00136688   -0.0036085  -0.00136688   0.00176618  -0.00136688 -0.000472579  0.000845606

However, I get a strange output when I print np.squeeze(fd.H()) instead (notice the first value is wrong):

❯ ./test.py
[[  1.44273176e-316  -9.29846512e-002   6.05445925e-002   4.62454099e-002
    2.89807543e-002   4.52922633e-004  -1.36687705e-003]
 [ -9.29846512e-002   2.55746886e+000  -9.16544126e-002  -1.06630280e+000
   -9.22458982e-002  -3.50883659e-003  -3.60849772e-003]
 [  6.05445925e-002  -9.16544126e-002   6.05445925e-002   4.62454099e-002
    2.89807543e-002   4.52922633e-004  -1.36687705e-003]
 [  4.62454099e-002  -1.06630280e+000   4.62454099e-002   5.80728820e-001
    4.64436817e-002  -7.90873565e-004   1.76618296e-003]
 [  2.89807543e-002  -9.22458982e-002   2.89807543e-002   4.64436817e-002
    2.89807543e-002   4.52922633e-004  -1.36687705e-003]
 [  4.52922633e-004  -3.50883659e-003   4.52922633e-004  -7.90873565e-004
    4.52922633e-004   1.82121924e-002  -4.72579279e-004]
 [ -1.36687705e-003  -3.60849772e-003  -1.36687705e-003   1.76618296e-003
   -1.36687705e-003  -4.72579279e-004   8.45606406e-004]]

However it's fine if I first convert fd.H() to a numpy array (e.g. np.squeeze(np.array(fd.H())). That might be because squeeze returns a view onto the array because something like the following is fine as well:

H = fd.H()
print(np.squeeze(H))

Can you reproduce the issue if you don't use np.squeeze in your case?

[albindgit]

It was indeed the issue with np.squeeze! All works fine when first converting to np.array as you mention. Thank you!