libfranka: Attempt to set invalid transformation in motion generator. Has to be column major

[Sophia-study]

Context
ubuntu: melodic
libfranka: 0.8.0
franka_ros: 0.8.0

Problem
Error launching cartesian_pose_example_controller.launch file, when I want to understand cartesian_pose_handle. I don't understand why this example I'm running is wrong, is there anything I haven't thought of?

[ INFO] [1679307925.856779009]: FrankaHW: Prepared switching controllers to cartesian_pose with parameters limit_rate=1, cutoff_frequency=100, internal_controller=joint_impedance
[INFO] [1679307925.862252]: Started controllers: cartesian_pose_example_controller
[INFO] [1679307925.866132]: Started controllers: franka_state_controller
terminate called after throwing an instance of 'std::invalid_argument'
what(): libfranka: Attempt to set invalid transformation in motion generator. Has to be column major!

[marcbone]

Thats weird. Is this reproducible? I looked through the implementation of the example and noticed some strange things that could cause issues. You can try to replace line 72 with:

Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(cartesian_pose_handle_->getRobotState().O_T_EE_d.data()));
initial_transform.linear() << initial_transform.rotation();
Eigen::Map<Eigen::Matrix4d>(&initial_pose_[0], 4, 4) = initial_transform.matrix();

You need to #include <Eigen/Dense> for this to work.
Please report back if this solves your problem

[Sophia-study]

That error still persists when I experiment with this change. I output the value of initial_pose_ under both methods.

initial_pose_ in the example
0.999918 0.00111351 -0.0015725 0 0.00104443 -0.999989 0.000966081 0 -0.00157155 -0.000967731 -0.999998 0 0.306823 0.00035104 0.486316 1

new initial_pose_
0.999999 0.000731824 -0.00111906 0 0.000732606 -0.999999 0.000698701 0 -0.00111854 -0.00069952 -0.999999 0 0.306881 0.000206699 0.486503 1

[marcbone]

Thanks for your reply. At least the new pose is homogeneous. I think if you go to this config file and set rate_limiting to false and the cutoff_frequency to 1000 it will work.

[Sophia-study]

Thanks a lot for your reply, it's working fine now.