Add camera projection sensor.

BEGIN_PUBLIC
Add camera projection sensor.
END_PUBLIC

PiperOrigin-RevId: 564726114
Change-Id: I33b8e5562eff29c21538bf8f38ce3116c7dfc5a9

src/user/user_model.cc

@@ -1586,6 +1586,7 @@ void mjCModel::CopyTree(mjModel* m) {
       copyvec(m->cam_quat+4*cid, pc->locquat, 4);
       m->cam_fovy[cid] = (mjtNum)pc->fovy;
       m->cam_ipd[cid] = (mjtNum)pc->ipd;
+      copyvec(m->cam_resolution+2*cid, pc->resolution, 2);
       copyvec(m->cam_user+nuser_cam*cid, pc->userdata.data(), nuser_cam);
     }
 
@@ -3044,6 +3045,7 @@ bool mjCModel::CopyBack(const mjModel* m) {
     copyvec(cameras[i]->quat, m->cam_quat+4*i, 4);
     cameras[i]->fovy = (double)m->cam_fovy[i];
     cameras[i]->ipd = (double)m->cam_ipd[i];
+    copyvec(cameras[i]->resolution, m->cam_resolution+2*i, 2);
 
     if (nuser_cam) {
       copyvec(cameras[i]->userdata.data(), m->cam_user + nuser_cam*i, nuser_cam);

src/user/user_objects.cc

@@ -1949,6 +1949,7 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) {
   fovy = 45;
   ipd = 0.068;
   userdata.clear();
+  resolution[0] = resolution[1] = 1;
 
   // clear private variables
   body = 0;
@@ -2003,6 +2004,12 @@ void mjCCamera::Compile(void) {
   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)",
+                   name.c_str(), id, fovy);
+  }
 }
 
 
@@ -4044,6 +4051,7 @@ void mjCSensor::Compile(void) {
   case mjSENS_TORQUE:
   case mjSENS_MAGNETOMETER:
   case mjSENS_RANGEFINDER:
+  case mjSENS_CAMPROJECTION:
     // must be attached to site
     if (objtype!=mjOBJ_SITE) {
       throw mjCError(this,
@@ -4054,19 +4062,32 @@ void mjCSensor::Compile(void) {
     if (type==mjSENS_TOUCH || type==mjSENS_RANGEFINDER) {
       dim = 1;
       datatype = mjDATATYPE_POSITIVE;
+    } else if (type==mjSENS_CAMPROJECTION) {
+      dim = 2;
+      datatype = mjDATATYPE_REAL;
     } else {
       dim = 3;
       datatype = mjDATATYPE_REAL;
     }
 
     // set stage
-    if (type==mjSENS_MAGNETOMETER || type==mjSENS_RANGEFINDER) {
+    if (type==mjSENS_MAGNETOMETER || type==mjSENS_RANGEFINDER || type==mjSENS_CAMPROJECTION) {
       needstage = mjSTAGE_POS;
     } else if (type==mjSENS_GYRO || type==mjSENS_VELOCIMETER) {
       needstage = mjSTAGE_VEL;
     } else {
       needstage = mjSTAGE_ACC;
     }
+
+    // check for camera resolution for camera projection sensor
+    if (type==mjSENS_CAMPROJECTION) {
+      mjCCamera* camref = (mjCCamera*) model->FindObject(mjOBJ_CAMERA, refname);
+      if (!camref->resolution[0] || !camref->resolution[1]) {
+        throw mjCError(this,
+                       "camera projection sensor requires camera resolution '%s' (id = %d)",
+                       name.c_str(), id);
+      }
+    }
     break;
 
   case mjSENS_JOINTPOS:

src/user/user_objects.h

@@ -465,6 +465,7 @@ class mjCCamera : public mjCBase {
   double ipd;                     // inter-pupilary distance
   double pos[3];                  // position
   double quat[4];                 // orientation
+  float resolution[2];            // resolution [pixel]
   std::vector<double> userdata;   // user data
   mjCAlternative alt;             // alternative orientation specification