From cbb048ff005ff956c51695b53b23309d5bc7c047 Mon Sep 17 00:00:00 2001
From: Yuval Tassa <tassa@google.com>
Date: Thu, 25 Apr 2024 02:09:32 -0700
Subject: [PATCH] Allow parent targeting for cameras and lights.

PiperOrigin-RevId: 628002614
Change-Id: I047dd3d62cfbc6432a3ec53fe6465afc52ca615b
---
 src/user/user_objects.cc       | 10 --------
 test/user/user_objects_test.cc | 46 ++++++++++++++++++++++++++++++++++
 2 files changed, 46 insertions(+), 10 deletions(-)

diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc
index 309e7c79..e7f3b589 100644
--- a/src/user/user_objects.cc
+++ b/src/user/user_objects.cc
@@ -2647,11 +2647,6 @@ void mjCCamera::Compile(void) {
     }
   }
 
-  // make sure it is not targeting parent body
-  if (targetbodyid==body->id) {
-    throw mjCError(this, "parent-targeting in camera '%s' (id = %d)", name.c_str(), id);
-  }
-
   // make sure the image size is finite
   if (fovy >= 180) {
     throw mjCError(this, "fovy too large in camera '%s' (id = %d, value = %d)",
@@ -2791,11 +2786,6 @@ void mjCLight::Compile(void) {
       throw mjCError(this, "unknown target body in light '%s' (id = %d)", name.c_str(), id);
     }
   }
-
-  // make sure it is not self-targeting
-  if (targetbodyid==body->id) {
-    throw mjCError(this, "parent-targeting in light '%s' (id = %d)", name.c_str(), id);
-  }
 }
 
 
diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc
index f77e52f5..c2777466 100644
--- a/test/user/user_objects_test.cc
+++ b/test/user/user_objects_test.cc
@@ -864,6 +864,52 @@ TEST_F(CameraSpecTest, FovyFromResolutionPixel) {
   mj_deleteModel(m);
 }
 
+TEST_F(CameraSpecTest, ParentTargetingNull) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom size="1"/>
+      <camera mode="targetbody" target="world"/>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, NotNull()) << error.data();
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+  // orientation can't be decided so we get an arbitrary but valid xmat
+  // (camera pointed towards the negative x-axis)
+  EXPECT_THAT(AsVector(data->cam_xmat, 9),
+              ElementsAre(0, 0, 1,
+                          1, 0, 0,
+                          0, 1, 0));
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
+TEST_F(CameraSpecTest, ParentTargeting) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <geom size="1"/>
+      <camera pos="1 1 0" mode="targetbody" target="world"/>
+    </worldbody>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  EXPECT_THAT(model, NotNull()) << error.data();
+  mjData* data = mj_makeData(model);
+  mj_forward(model, data);
+  // expect negative z-axis to point from camera to world
+  EXPECT_FLOAT_EQ(data->cam_xmat[2], mju_sqrt(0.5));
+  EXPECT_FLOAT_EQ(data->cam_xmat[5], mju_sqrt(0.5));
+  EXPECT_FLOAT_EQ(data->cam_xmat[8], 0);
+  mj_deleteData(data);
+  mj_deleteModel(model);
+}
+
 // ------------- test actuator order -------------------------------------------
 
 using ActuatorTest = MujocoTest;