ValueError: libfranka: Attempt to set invalid transformation in motion generator. Has to be column major!
Opened this issue · 11 comments
I'm using frankx (a high level api for libfranka), but when I run example/home.py i get:
# python3 examples/home.py
initial position: 0.529378
0.123311
0.37398
2.35987
-0.0296578
0.0611505
-0.643318
initial velocity: 0
0
0
0
0
0
0
initial acceleration: 0
0
0
0
0
0
0
target position: 0.4805
-0.229
0.389
0
-0
0
1.75
target velocity: 0
0
0
0
0
0
0
tf: 4298.34
v_max_tfs: 71.8799
518.105
22.0886
1179.93
14.8289
30.5753
4298.34
a_max_tfs: 8.5064
22.8376
4.71548
30.1382
3.37865
4.85148
84.8339
j_max_tfs: 1.55498
3.00373
1.04933
3.61387
0.840214
1.06941
7.20455
a: -1.99876e-19
-1.44069e-18
6.14216e-20
-4.04976e-12
-6.29602e-12
-4.05035e-12
-6.29601e-12
b: 2.14785e-15
1.54815e-14
-6.60028e-16
3.48145e-08
5.4125e-08
3.48196e-08
5.41248e-08
c: -6.15479e-12
-4.43632e-11
1.89135e-12
-7.48225e-05
-0.000116324
-7.48333e-05
-0.000116324
d: 0
0
0
0
0
0
0
e: 0
0
0
0
0
0
0
f: 0.529378
0.123311
0.37398
2.35987
-0.0296578
0.0611505
-0.643318
Traceback (most recent call last):
File "examples/home.py", line 17, in <module>
robot.move(camera, LinearMotion(home, 1.75))
ValueError: libfranka: Attempt to set invalid transformation in motion generator. Has to be column major!
To reproduce:
git clone https://github.com/dti-research/frankx.git
cd frankx
docker build -t pantor/frankx -f docker/Dockerfile .
docker run -it --rm --network=host --privileged -v $(pwd):/code/frankx -w /code/frankx pantor/frankx
And inside the container build the repo:
mkdir build && cd build
cmake ..
make -j$(nproc)
make install
And finally run the example:
python3 example/home.py
I've checked that the transformation matrix is correct, which it seems it is.
I'm running firmware 4.0.4 and libfranka 0.8.0
Any updates on this?
Hi,
the transformation matrix set in the settings looks good!
The error you get arises here in libfranka, so on client side. This check is done for the commanded O_T_EE before it is sent to the robot. Please make sure to disable rate-limiting and filtering in the control loop. To debug that error further, you could log the commanded O_T_EE and check if it is a valid homogeneous transformation.
In 0.8.0 we split up F_T_EE into F_T_NE (which can be set in DESK) and NE_T_EE (can be set during run-time by the setEE operation) but also provide the user a calculated F_T_EE in the robot state. See here for all 0.8.0 changes.
Okay. I will look into this at a later date. I would strongly recommend that you find a way to allow users to downgrade the controller firmware to whatever the user want.
Hi @sgabl. So the commanded O_T_EE looks like this:
0.707109 -0.707104 7.79036e-07 0.480498
0.707104 0.707109 1.43685e-06 -0.204002
-1.56686e-06 -4.65148e-07 1 0.2676
0 0 0 1
Which seems to be a valid homogeneous transformation.
I've inserted a print statement just before control_tools.h#L54 to verify that libfranka indeed thinks it is a homogeneous transformation. Here I'm also printing out my transformation in the array form. This results in:
Libfranka: Transformation is homogeneous!
0.707113 0.707101 -1.56686e-06 0 -0.707101 0.707113 -4.65155e-07 0 7.79036e-07 1.43685e-06 1 0 0.480498 -0.204002 0.2676 1
And this should only be printed when the transform is homogeneous. But wierdly enough, calling the isHomogeneousTransformation() function from inside the checkMatrix results in:
isHomogeneousTransformation(transform) returned: 0
What is going on?
Hi @nily-dti. Yes, the commanded O_T_EE you posted is a valid homogeneous transformation and won't be rejected by libfranka.
To debug that problem further I think that a good place to insert a print statement would be before control_loop.cpp#L47 or as a part of the invalid_argument message. When printing in every control loop step (1kHz), you could get side effects like lost / late command packets which could change the behavior.
Met the same issues, any updates?
The problem arises because frankx tries to set EE matrix in set_default_behavior(). However the behavior of setEE() function changes in libfranka 0.8.0.
The solution is to never call set_default_behavior() and instead set the flange to end effector matrix in desk to the value specified in frankx's cpp implementation of set_default_behavior().
However you will then discover that frankx runs into acceleration discontinuity problem really often with LinearMotion despite having set a low max velocity/acceleration/jerk so you cannot expect frankx to move your robot in a smooth fashion using this solution.
Another solution might be to modify the motion_waypoint_generator.cpp to feed the Ruckig motion planner flange pose through the recommended F_T_EE matrix in libfranka 0.8.0 and firmware version 4. However, I have not tried this solution.
Hi, @jmkl009
When I was trying to run the examples that use the cartesian motion generator, the exceptions were thrown by libfranka (e.g. cartesian_motion_generator_joint_acceleration_discontinuity, cartesian_motion_generator_joint_velocity_discontinuity,). So, could you please share the solutions if you have already solved the similar issues?
Best regards
Hi, @jmkl009
When I was trying to run the examples that use the cartesian motion generator, the exceptions were thrown by libfranka (e.g. cartesian_motion_generator_joint_acceleration_discontinuity, cartesian_motion_generator_joint_velocity_discontinuity,). So, could you please share the solutions if you have already solved the similar issues?
Best regards
I have not solved the issue of acc discontinuity using LinearMotion/WaypointMotion in frankx. This problem arises due to Ruckig not checking joint acceleration against the limits when interpolating in cartesian space. We however cannot check the joint limits for Ruckig because that require us to have access to the internal inverse kinematic solver of the panda arm. That being said, I have heard people trying to convert CartesianPose to corresponding CartesianVelocity control and had success solving the problem. Another possible solution is to have ruckig plan out inside a motion generator and we implement PD control to achieve the pose at every iteration.