strange behaviour when running mpc example

[stiefen1]

Hello,

After following the setup instructions without errors, I got an error while running "convex_mpc_controller_example.py" about the type of the argument provided to the "compute_contact_forces" function, but I didn't changed anything in the code!
I've just run this command (provided in your repo) : python -m src.convex_mpc_controller.convex_mpc_controller_example --show_gui=True --max_time_secs=10 --world=plane from the main folder.

pybullet build time: May 20 2022 19:44:17
argv[0]=
startThreads creating 1 threads.
starting thread 0
started thread 0
argc=3
argv[0] = --unused
argv[1] =
argv[2] = --start_demo_name=Physics Server
ExampleBrowserThreadFunc started
X11 functions dynamically loaded using dlopen/dlsym OK!
X11 functions dynamically loaded using dlopen/dlsym OK!
Creating context
Created GL 3.3 context
Direct GLX rendering context obtained
Making context current
GL_VENDOR=NVIDIA Corporation
GL_RENDERER=NVIDIA GeForce GT 1030/PCIe/SSE2
GL_VERSION=3.3.0 NVIDIA 525.78.01
GL_SHADING_LANGUAGE_VERSION=3.30 NVIDIA via Cg compiler
pthread_getconcurrency()=0
Version = 3.3.0 NVIDIA 525.78.01
Vendor = NVIDIA Corporation
Renderer = NVIDIA GeForce GT 1030/PCIe/SSE2
b3Printf: Selected demo: Physics Server
startThreads creating 1 threads.
starting thread 0
started thread 0
MotionThreadFunc thread started
ven = NVIDIA Corporation
ven = NVIDIA Corporation
I0405 09:33:06.039097 140688990267200 locomotion_controller.py:294] Switched to Crawling gait.
I0405 09:33:06.039403 140688627767040 locomotion_controller.py:310] Low level thread started...
I0405 09:33:06.039460 140688627767040 locomotion_controller.py:248] Standing up.
I0405 09:33:06.338073 140688627767040 locomotion_controller.py:297] Switched to Trotting gait.
/home/smonnet/Desktop/quadruped-sim-master/fast_and_efficient/src/convex_mpc_controller/com_velocity_estimator.py:52: 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.
normal_vec = np.linalg.lstsq(contact_foot_positions, np.ones(4))[0]
I0405 09:33:06.341569 140688627767040 locomotion_controller.py:251] Walking.
Exception in thread Thread-1:
Traceback (most recent call last):
File "/usr/lib/python3.8/threading.py", line 932, in _bootstrap_inner
self.run()
File "/usr/lib/python3.8/threading.py", line 870, in run
self._target(*self._args, **self._kwargs)
File "/home/smonnet/Desktop/quadruped-sim-master/fast_and_efficient/src/convex_mpc_controller/locomotion_controller.py", line 322, in run
action, qp_sol = self.get_action()
File "/home/smonnet/Desktop/quadruped-sim-master/fast_and_efficient/src/convex_mpc_controller/locomotion_controller.py", line 211, in get_action
stance_action, qp_sol = self._stance_controller.get_action()
File "/home/smonnet/Desktop/quadruped-sim-master/fast_and_efficient/src/convex_mpc_controller/torque_stance_leg_controller_mpc.py", line 174, in get_action
predicted_contact_forces = self._cpp_mpc.compute_contact_forces(
TypeError: compute_contact_forces(): incompatible function arguments. The following argument types are supported:
1. (self: mpc_osqp.ConvexMpc, arg0: List[float], arg1: List[float], arg2: List[float], arg3: List[float], arg4: List[int], arg5: List[float], arg6: List[float], arg7: List[float], arg8: List[float], arg9: List[float], arg10: List[float]) -> List[float]

Invoked with: <mpc_osqp.ConvexMpc object at 0x7ff4a92335f0>, array([0. , 0. , 0.23861794]), array([ 4.42300643e-06, -1.66188485e-08, 1.05292600e-06]), array([-0.00122399, -0.01933636, 0. ]), array([-4.02889509e-06, 2.06527659e-07, 1.00000000e+00]), [0, 0, 0], array([0., 1., 1., 0., 0., 1., 1., 0., 0., 1., 1., 0., 0., 1., 1., 0., 0.,
1., 1., 0., 0., 1., 1., 0., 0., 1., 1., 0., 0., 1., 1., 0., 0., 1.,
1., 0., 0., 1., 1., 0.]), array([ 0.17128076, -0.13418603, -0.2422553 , 0.1719579 , 0.12992437,
-0.24194507, -0.1778831 , -0.13415863, -0.23550281, -0.17788233,
0.12993178, -0.23517963]), array([0.6, 0.6, 0.6, 0.6]), array([0. , 0. , 0.3]), array([0.09, 0. , 0. ]), array([0., 0., 0.]), array([0., 0., 0.])

I would really appreciate if you have any idea about the reason why I get this error ! I'm using Python 3.8.10 and Ubuntu 20.04.

Thanks a lot,
Stephen

[yxyang]

Not exactly sure why this is happening. I just did a fresh-clone of the repo on Python 3.8.10 + Ubuntu 20.04 and the code runs fine.

It seems that there is some issue with the base angular velocity, which is interpreted as list[int] but should be list[float]. The angular velocity should always be a list[float] though. Maybe try casting it as float and give another try?

[stiefen1]

I've tried to cast it but it seems that no matter what I provide to the function (even a constant float list : [0., 0., 0.]) I got the same error. Could it be the version of any library I'm using ? Because when I'm installing the libraries I got an error about the version of numpy which is not compatible with that of tensorflow (I don't need tensorflow since I only want to use your mpc controller).

ERROR: tensorflow 2.12.0 has requirement numpy<1.24,>=1.22, but you'll have numpy 1.24.2 which is incompatible.

Thanks a lot,
Stephen

[yxyang]

Doesn't look like a numpy issue to me --- I tested the MPC with numpy 1.24.2 and it works fine as well.

It is a really weird error that I have not seen before... Can you double check that you have not modified the code in any unintended ways, and re-compile the C++ binaries?

If it still doesn't work, you can try the following python implementation of the same MPC algorithm. I recently wrote it, but have not tested it thoroughly yet. It should follow a similar interface compared to the current repo.

mpc_python.zip

[stiefen1]

Hello,

I've simply recompiled the binaries and for some reasons now it works ! Thanks a lot for your help!

Best,
Stephen