fix: use correct site rotation for laser scans

the rotation matrix was accidentally transposed before multiplication.
ubi-agni · Sep 19, 2024 · 9da4ff1 · 9da4ff1
1 parent 2cecaf8
commit 9da4ff1
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/mujoco_ros_laser/src/laser.cpp b/mujoco_ros_laser/src/laser.cpp
@@ -197,7 +197,7 @@ void processRay(const mjModel *model, mjData *data, std::mt19937 rand_generator,
 {
 	mjtNum pos[3], target[3];
 	mju_copy(pos, laser_config.rays + 3 * ray_idx, 3);
-	mju_mulMatTVec3(target, rot, pos);
+	mju_mulMatVec3(target, rot, pos);
 
 	// start position
 	mju_copy(pos, data->site_xpos + 3 * laser_config.site_attached, 3);