def getSim(simfile):
if simfile in simTable:
return simTable[simfile][1]
world = klampt.WorldModel()
if not world.readFile(simfile):
raise ValueError("Invalid simulation file " + simfile)
sim = klampt.Simulator(world)
simTable[simfile] = (world, sim)
return sim
mind = min(depth,mind)
maxd = max(depth,maxd)
print "Depth range",mind,maxd
"""
rgb, depth = sensing.camera_to_images(sensor)
return rgb, depth
world = klampt.WorldModel()
world.readFile("../../data/simulation_test_worlds/sensortest.xml")
#world.readFile("../../data/tx90scenario0.xml")
robot = world.robot(0)
vis.add("world", world)
sim = klampt.Simulator(world)
sensor = sim.controller(0).sensor("rgbd_camera")
print("sensor.getSetting('link'):", sensor.getSetting("link"))
print("sensor.getSetting('Tsensor'):", sensor.getSetting("Tsensor"))
input("Press enter to continue...")
#T = (so3.sample(),[0,0,1.0])
T = (so3.mul(so3.rotation([1, 0, 0], math.radians(-10)),
[1, 0, 0, 0, 0, -1, 0, 1, 0]), [0, -2.0, 0.5])
sensing.set_sensor_xform(sensor, T, link=-1)
vis.add("sensor", sensor)
read_local = True
#Note: GLEW sensor simulation only runs if it occurs in the visualization thread (e.g., the idle loop)
def transform_to_string(R, t):
rv = " ".join(map(str, R)) + " ".join(map(str, t))
return rv
# add table + robot
builder = testingWorldBuilder(30, 30)
builder.addTableTopScenario(x=1.3, y=-0.25)
builder.addRobot("./Anthrax.urdf", None)
w = builder.getWorld()
robot = w.robot(0)
reset_arms(robot)
sim = klampt.Simulator(w)
# setup simulated camera
cam = klampt.SimRobotSensor(sim.controller(0), "rgbd_camera", "CameraSensor")
# mount camera in place
cam.setSetting("link", "4")
#cam.setSetting("Tsensor","0.0 -0.866 -0.5 1.0 0.0 0.0 0 -0.5 0.866 0.2 0.005 1.2")
cam.setSetting(
"Tsensor",
transform_to_string(so3.from_rpy((math.pi, radians(-30), radians(270))),
[0.2, 0.005, 1.2]))
# minimum range
cam.setSetting("zmin", "0.1")
# x field of view
cam.setSetting("xfov", "1.5")