def setUp(self):
add_object(graphic_file="duck.obj", collision_file="duck_vhacd.obj", base_position=[1., 0., 0.2],
base_orientation=[0., 0., 0., 1.], mesh_scale=[.1, .1, .1], COM_shift=[0, 0.04, 0.])
self.cam = Camera(width=640, height=480)
self.cam.set_view_matrix(camera_eye_pos=[2, 0, 0.5], camera_target_pos=[0, 0, 0.5], camera_up_vec=[0, 0, 1])
self.cam.set_projection_matrix(fovy=60, aspect=1., near=0.5, far=10)
# set initial position and orientation
position = np.array([0., 0., 1.3])
orientation = np.array([0, 0, 0, 1])
base_pose = geometry.list2pose_stamped(list(position) + list(orientation))
T = transformations.euler_matrix(0, 0, 0)
pose_transform = geometry.pose_from_matrix(T, frame_id='body')
object_pose = geometry.transform_body(base_pose, pose_transform)
object_pose_list = geometry.pose_stamped2list(object_pose)
# add object
obj_id = add_object(
graphic_file=info['obj'],
collision_file=info['obj'],
mass=.5,
base_position=object_pose_list[0:3],
base_orientation=object_pose_list[3:7],
mesh_scale=info['scale'],
color=[1, 0, 0, 1],
)
data = defaultdict(lambda: [])
for t in range(args.n_timesteps):
if (t + 1) % args.interval == 0:
rgb_img, rgb_equilibrium, depth_equilibrium, seg_img, seg_equilibrium = sensor.get_sensor_image(
)
seg_img = np.where(seg_img != obj_id, -1, obj_id)
pointcloud = sensor.get_sensor_pointcloud(rgb_equilibrium,
def __init__(self,
position,
orientation,
mesh_scale,
sensor_vector,
mass=10000,
camera_up_vector=(0, 1, 0),
image_width=640,
image_height=480,
camera_fovy=60,
camera_aspect=1,
camera_near=0.01,
camera_far=1,
simple_model=False,
constrained=False,
virtual_links=False):
"""
Args:
position (list) : Initial position of the COM of the sensor in world coordinates.
orientation (list) : Initial orientation of the COM of the sensor in world coordinates.
mesh_scale (list) : Scale of the sensor OBJ. Sensor size is equivalent to this if simple_model=True.
sensor_vector (list) : The vector pointing towards the facing direction of the camera in the local frame
camera_up_vector (list) : Up vector of the camera
image_width (int) : Image width
image_height (int) : Image height
camera_fovy (float) : The vertical lens opening angle.
camera_aspect (float) : The aspect ratio (width/height) of the lens.
camera_near (float) : Distance to the front clipping plane.
camera_far (float) : Distance to the back clipping plane.
simple_model (bool) : If true, uses a simple cube as the sensor.
"""
self._position = np.array(position)
self._orientation = np.array(orientation)
self._sensor_size = np.array(mesh_scale) if simple_model else np.array(
[1.6, 1.6, .5])
self._init_sensor_vector = sensor_vector
self._time = 0.
self._virtual_links = virtual_links
self._constrained = constrained
self._max_force = 10000
model = Path(os.path.dirname(
os.path.realpath(__file__))).parents[1].joinpath(
'graphics', 'descriptions', 'sensor',
'tactile_sensor.obj') if not simple_model else "cube.obj"
self._sensor_id = add_object(
graphic_file=model,
collision_file=model,
base_position=position,
base_orientation=orientation,
mesh_scale=mesh_scale,
mass=mass,
color=[x / 255 for x in [255, 157, 0, 256]],
virtual_links=virtual_links)
if constrained:
self._sensor_constraint = p.createConstraint(
parentBodyUniqueId=self._sensor_id,
parentLinkIndex=-1,
childBodyUniqueId=-1,
childLinkIndex=-1,
jointType=p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, 0],
childFrameOrientation=[0, 0, 0])
self._camera = Camera(width=image_width,
height=image_height,
camera_up_vector=camera_up_vector)
self._camera.set_projection_matrix(fovy=camera_fovy,
aspect=camera_aspect,
near=camera_near,
far=camera_far)
# surface normal vector and spanning vectors
surface_vectors = [0 if x == 1 else 1 for x in sensor_vector]
self._init_surface_vec_1, self._init_surface_vec_2 = np.zeros(
3), np.zeros(3)
self._init_surface_vec_1[np.nonzero(surface_vectors)[0][0]] = 1
self._init_surface_vec_2[np.nonzero(surface_vectors)[0][1]] = 1
self._sensor_vector, self._surface_vec_1, self._surface_vec_2 = np.array(
[]), np.array([]), np.array([])
# place holder for debug lines
self.debug_line = []
for i in range(5):
self.debug_line.append(
p.addUserDebugLine([0., 0., 0.], [1., 0., 0.], [1, 0, 0]))
[0, 0, info['mesh_height'] / 4])
# COM_shift = info['center_mass']
print(COM_shift)
position, orientation = sample_pose(init_pos,
random_chance=0.8,
random_orn=True,
gaussian_mean=0,
gaussian_std=0.05)
obj_id = add_object(
graphic_file=info['obj'],
collision_file=info['obj'],
# mass=info['weight'],
mass=1,
base_position=init_pos - info['center_mass'],
base_orientation=[0, 0, 0, 1],
mesh_scale=[info['scale']] * 3,
COM_shift=COM_shift,
color=color,
)
# quick hack for better falling dynamics
inertial = np.array((p.getDynamicsInfo(obj_id, -1))[2]) / 5
p.changeDynamics(
obj_id,
-1,
# localInertiaDiagonal=inertial.tolist(),
rollingFriction=0.005,
# restitution=1,
contactStiffness=2000,
setup_pybullet(time_step=config.TIME_STEP,
renders=not args.headless,
gravity=True)
sensor = make_sensor(size=[1, 1, 1],
position=[0, 0, 1],
sensor_vector=[0, 0, 1],
orientation=p.getQuaternionFromEuler(
[slope, 0, 0]),
thickness=0.005,
use_force=False,
constrained=True)
add_object(
graphic_file="cube.obj",
collision_file="cube.obj",
base_position=[0, -1.7, 2],
base_orientation=[0, 0, 0, 1],
mesh_scale=[2, 2, 4],
color=[x / 255 for x in [199, 193, 193, 100]],
mass=10000,
)
img_counter = 0
print("Index {}".format(index))
print(
"OBJ #{} - {}: Collecting images from the object {} from category {}"
.format(total_counter, k + 1, info['obj_name'],
info['category']))
# if the object has no image-based texture, pick one color randomly
if not info['textured_material']:
color = random.choice(info['colors'])