def __init__(self, env, world_tf_id):
self.env = env
self.world_tf_id = world_tf_id
self.tfplugin = orpy.RaveCreateSensor(
self.env, 'tfplugin tfplugin ' + self.world_tf_id)
env.Add(self.tfplugin)
def BHTactileSensor(env, node_name, namespace, robot, link_prefix):
args = [
'BHTactileSensor', node_name, namespace,
robot.GetName(), link_prefix
]
args_str = ' '.join(args)
module = openravepy.RaveCreateSensor(env, args_str)
if module is None:
raise Exception('Creating BHTactileSensor failed.')
env.Add(module, True)
return Bind(module)
def __init__(self, env, transform=None,
fx=529, fy=525, cx=328, cy=267, near=0.01, far=10.0,
width=640, height=480):
"""Initializes the camera with intrinsics fx, fy, cx, cy, with a near
and far plane (in meters) with width and height in pixels"""
self.env = env;
self.lock = Lock()
self.camera = openravepy.RaveCreateSensor(env, 'offscreen_render_camera')
self.set_intrinsic(fx, fy, cx, cy, near, far, width, height)
self.camera.Configure(openravepy.Sensor.ConfigureCommand.PowerOn);
if (transform is None):
self.set_transform(numpy.eye(4))
else:
self.set_transform(transform)
def render_frame_thread(camera_sim):
info = camera_sim.camera_info;
camera_sim.camera = openravepy.RaveCreateSensor(camera_sim.env, 'rave_to_ros_camera');
camera_sim.send_command('setintrinsic ' + str(info.K[0]) + ' ' + str(info.K[4]) + ' ' + str(info.K[2]) + ' ' + str(info.K[5]) + ' ' + str(camera_sim.near) + ' ' + str(camera_sim.far));
camera_sim.send_command('setdims ' + str(info.width) + ' ' + str(info.height));
camera_sim.send_command('initialize ~ ' + camera_sim.namespace + '/sim_depth' + ' ' + camera_sim.namespace + '/sim_color ' + camera_sim.namespace + '/sim_points ' + info.header.frame_id + '_sim ' + camera_sim.fixed_frame);
camera_sim.camera.Configure(openravepy.Sensor.ConfigureCommand.PowerOn);
while (True):
time.sleep(0.016);
with camera_sim.lock:
camera_sim.camera.SimulationStep(0.01);
for add in camera_sim.additions:
camera_sim.send_command('addbody ' + add[0] + ' ' + str(add[1]) + ' ' + str(add[2]) + ' ' + str(add[3]));
camera_sim.additions = [];
for rem in camera_sim.removals:
camera_sim.send_command('removebody ' + rem);
camera_sim.removals = [];
pass;
def __init__(self, env, world_tf_id):
self.env = env
self.world_tf_id = world_tf_id
self.or_tf = orpy.RaveCreateSensor(self.env,'or_tf or_tf '+self.world_tf_id)
env.Add(self.or_tf)
from openravepy import *
env = Environment() # create openrave environment
# Load some objects and set them up in the environment
env.Load('/home/mklingen/prdev/src/pr-ordata/data/objects/bowl.kinbody.xml')
env.Load(
'/home/mklingen/prdev/src/pr-ordata/data/objects/fuze_bottle.kinbody.xml')
bowl = env.GetKinBody('bowl')
fuze = env.GetKinBody('fuze_bottle')
tf = fuze.GetTransform()
tf[0:3, 3] = numpy.array([0.3, 0, 0.5])
fuze.SetTransform(tf)
# Create the sensor
sensor = openravepy.RaveCreateSensor(env, 'offscreen_render_camera')
# Set its intrinsics (fx, fy, cx, cy, near, far)
sensor.SendCommand('setintrinsic 529 525 328 267 0.01 10')
# And its resolution in pixels
sensor.SendCommand('setdims 640 480')
#Initialize the sensor. Right now the size of the sensor can't be changed after you do this.
# It will also open up an annoying opengl window just to get context.
sensor.Configure(openravepy.Sensor.ConfigureCommand.PowerOn)
# Add bodies to render with the given (r, g, b) colors
sensor.SendCommand('addbody bowl 255 0 0')
sensor.SendCommand('addbody fuze_bottle 0 255 0')
# You can also set the sensor's extrinsic transform
tf[0:3, 3] = numpy.array([0.0, 0, -0.5])
sensor.SetTransform(tf)