fix: use correct site rotation for laser scans

DavidPL1 · web-flow · commit 9da4ff15e3f0 · 2024-09-19T14:37:02.000+02:00
the rotation matrix was accidentally transposed before multiplication.
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);

Original file line number	Diff line number	Diff line change
`@@ -197,7 +197,7 @@ void processRay(const mjModel model, mjData data, std::mt19937 rand_generator,`
`197`	`197`	`{`
`198`	`198`	`mjtNum pos[3], target[3];`
`199`	`199`	`mju_copy(pos, laser_config.rays + 3 * ray_idx, 3);`
`200`		`- mju_mulMatTVec3(target, rot, pos);`
	`200`	`+ mju_mulMatVec3(target, rot, pos);`
`201`	`201`
`202`	`202`	`// start position`
`203`	`203`	`mju_copy(pos, data->site_xpos + 3 * laser_config.site_attached, 3);`