Issues with kinematicSimulate of sensors

[smeng9]

• [x] In the point click grasping simulation, we want to capture rgbd images from simulated camera sensor. We are following the document http://motion.cs.illinois.edu/software/klampt/latest/pyklampt_docs/klampt.robotsim.html?highlight=kinematicsimulate#klampt.SimRobotSensor to simulate the pose of the sensor before capturing We found out the kinematicSimulate of sensor does not update the pose correctly, the camera sensor is always returning the initial pose, so it cannot be moved around when the robot joint state is updated. Currently we switch back to whole world simulation as a contingency plan.

  I have attached a minimal case in the zip file that isolates the problem sensorsimulate_minimumcase.zip In the simulation it will print out sensor transform which is a constant.

• [ ] Also there are multiple potential integer overflows when modifying urdf file while the sensors link reference is not changed in xml. We probably need to check for bounds during parsing here https://github.com/krishauser/Klampt/blob/4779211b7368f10a4f1daf19139bcc8321cd8356/Cpp/Modeling/Robot.cpp#L182

  So the problems do not carry down below in the functions calling the method. Some of the return values are checked while some are not. https://github.com/krishauser/Klampt/blob/4779211b7368f10a4f1daf19139bcc8321cd8356/Cpp/Modeling/Robot.cpp#L3152 https://github.com/krishauser/Klampt/blob/7e34b5161958b53de51e4acb6cf1b642f73a6437/Cpp/IO/XmlWorld.cpp#L550 https://github.com/krishauser/Klampt/blob/34a6f2fe3b96671653a951689e80aa8ea5f7a2da/Cpp/Sensing/Sensor.cpp#L358

[krishauser]

The problem is that you are calling sim.updateWorld(), which modifies all of the state (configurations and velocities) of robots and objects in the world model to match the simulation. Since you aren't moving anything in the simulation, everything gets reset to the initial values.

What's happening is

0. You initialize the simulator
1. You set the robot model's configuration
2. You simulate the image, which should be correctly updated
3. You DONT advance the simulation (actually nothing is really driving the simulated robot anyway)
4. You ask the simulation to update the world model, which then changes the robot's configuration back to what it was in step 0
5. You ask for the sensor transform, and the robot is back to where it was at step 0, so it doesn't change.

The simple solution is to take out step 4.

As for the integer bounds checking, yes, that's generally something that should be done more often when reading from files. Would you like to volunteer to add those checks?

[smeng9]

I see. Dropping the updateWorld works, Thank you. That means if we are running pure kinematicSimulation without a controller it does not have anything to do with the world.

I will create a patch for all these checks.

[krishauser]

Not quite. Kinematicsimulate has nothing to do with the simulator (except when the simulator modifies the world)

[krishauser]

Closed due to inactivity -- issue seems to be fixed.