Problem with YdLidar frame

Hi,

I have a problem with the isaac.ydlidar.YdLidar component when using if for the local map - it is reporting the objects found on the right side to the left side of the robot (and vice versa). I tried to Initialise the LiDAR Pose as:

“lidar_pose”: {
“initial_pose”: {
“lhs_frame”: “robot”,
“rhs_frame”: “lidar”,
“pose”: [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
}
}
Which I think it should do a 180 degrees roll but then it will crash here:
// Creates rotation from a not necessarily normalized direction vector
static SO2 FromDirection(const Vector2& direction) {
SO2 q;
const K norm = direction.norm();
ASSERT(!IsAlmostZero(norm), “Direction vector must not be 0”);
q.cos_sin_ = direction / norm;
return q;
}
From so2.hpp

I tried to do rotations around z-coordinate (yaw) and they work as expected. Roll (rot around x) and pitch (rot around y) bot crash the Isaac engine.

Regards,
Andrei

I believe what you want is a pose of [0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 0.0]. This will give you a rotation of 180 degrees with no translation.

The pose parameters seem to be as follows: qw, qx, qy, qz, tx, ty, tz. The rotation is stored as quaternions and appear to be converted to 2d rotation for the pose. If you want to see source code for the rotation and equations, check “engine/core/math/so3.hpp” for “toSO2XY”.

Thanks, the problem seems to be in “engine/core/math/so3.hpp” for “toSO2XY”

They create a rotation matrix from a quaternion like this:
// 2D rotation matrix:
// cos(a) -sin(a)
// sin(a) cos(a)
// Quaternion to 3D rotation matrix:
// 1 - 2*(qy^2 + qz^2) 2*(qxqy - qzqw) …
// 2*(qxqy + qzqw) 1 - 2*(qx^2 + qz^2) …
// It follows (modulo re-normalization):
// cos(a) = 1 - (qx^2 + qy^2 + 2qz^2)
// sin(a) = 2
qz*qw
// These formulas correspond to the half-angle formulas for sin/cos.
And for a roll in 3D (qw=0, qx=1, qy=0, qz=0) the rotation matrix would be:
[ 1.0000000, 0.0000000, 0.0000000;
0.0000000, -1.0000000, 0.0000000;
0.0000000, 0.0000000, -1.0000000 ]
cos(a) = 0
sin(a) = 0?
Makes no sense as a rotation.
I suppose there is no way to transform any 3D rotation into a 2D one. I think the correct approach would be to compute everything as 3D vectors and then project to the XY plane.

BTW I did try it with [0,1,1,0] and was giving readings at an angle, not flipped around X.
Actually that quaternion should be [ 0, 0.7071068, 0.7071068, 0 ] (normalised)

you should use SO3::FromAxisAngle

I did try it, same thing. It just converts to a quaternion anyway:

static SO3 FromAngleAxis(K angle, const Vector3& axis) {
return SO3(Quaternion(Eigen::AngleAxis(angle, axis.normalized())));
}

I did solve it by redirecting the flatscan through a codelet and changing the angle there.

Just for anyone else trying to set pose3 in conf json, you can specify rotation without seeing quaternions. Check https://docs.nvidia.com/isaac/isaac/doc/faq.html. Example in-case they remove the section in the future

{
   "translation": [25.0, 20.0, 0.0],

   "rotation": { "yaw_degrees": 180 }
}

instead of the square brackets above. Likely won’t solve the original problem however, but you’ve done it already :)