def _get_packaged_pose(self, mesh):
# Make copy of argument
mesh = mesh.copy()
# Compute stable pose with least z-axis height
tfs, _ = mesh.compute_stable_poses()
min_z, min_tf = np.infty, None
for tf in tfs:
m = mesh.copy().apply_transform(tf)
z_ext = m.extents[2]
if z_ext < min_z:
min_z = z_ext
min_tf = tf
# Put mesh in that stable pose
mesh.apply_transform(min_tf)
# Rotate the mesh about that stable pose, pick one that gives largest y extent
theta = 0
dtheta = 0.1
min_rot = np.eye(4)
min_x_ext = mesh.extents[0]
while theta < np.pi:
rot = np.eye(4)
rot[:3, :3] = RigidTransform.z_axis_rotation(theta)
m = mesh.copy()
m.apply_transform(rot)
x_ext = m.extents[0]
if x_ext < min_x_ext:
min_x_ext = x_ext
min_rot = rot
theta += dtheta
return min_rot.dot(min_tf)
def __eq__(self, other):
""" Check equivalence by rotation about the z axis """
if not isinstance(other, StablePose):
raise ValueError('Can only compare stable pose objects')
R0 = self.r
R1 = other.r
dR = R1.T.dot(R0)
theta = 0
while theta < 2 * np.pi:
Rz = RigidTransform.z_axis_rotation(theta)
dR = R1.T.dot(Rz).dot(R0)
if np.linalg.norm(dR - np.eye(3)) < 1e-5:
return True
theta += d_theta
return False
def rotate_callback(viewer, key, rot, stf):
rot.rotation = RigidTransform.z_axis_rotation(np.pi / 2.0).dot(
rot.rotation)
vis.get_object(key).T_obj_world = rot.dot(stf)
def sample(self):
""" Samples a state from the space
Returns
-------
:obj:`HeapState`
state of the object pile
"""
# Start physics engine
self._physics_engine.start()
# setup workspace
workspace_obj_states = []
workspace_objs = self._config['workspace']['objects']
for work_key, work_config in list(workspace_objs.items()):
# make paths absolute
mesh_filename = work_config['mesh_filename']
pose_filename = work_config['pose_filename']
if not os.path.isabs(mesh_filename):
mesh_filename = os.path.join(
os.path.dirname(os.path.realpath(__file__)), '..',
mesh_filename)
if not os.path.isabs(pose_filename):
pose_filename = os.path.join(
os.path.dirname(os.path.realpath(__file__)), '..',
pose_filename)
# load mesh
mesh = trimesh.load_mesh(mesh_filename)
mesh.density = self.obj_density
pose = RigidTransform.load(pose_filename)
workspace_obj = ObjectState(work_key, mesh, pose)
self._physics_engine.add(workspace_obj, static=True)
workspace_obj_states.append(workspace_obj)
# sample state
train = True
if np.random.rand() > self._train_pct:
train = False
sample_keys = self.test_keys
self._logger.info('Sampling from test')
else:
sample_keys = self.train_keys
self._logger.info('Sampling from train')
total_num_objs = len(sample_keys)
# sample object ids
num_objs = min(self.num_objs_rv.rvs(size=1)[0], total_num_objs - 1) + 1
num_objs = min(num_objs, self.max_objs)
num_objs = max(num_objs, self.min_objs)
obj_inds = np.random.permutation(np.arange(total_num_objs))
# log
self._logger.info('Sampling %d objects' % (num_objs))
# sample pile center
heap_center = self.heap_center_space.sample()
t_heap_world = np.array([heap_center[0], heap_center[1], 0])
self._logger.debug('Sampled pile location: %.3f %.3f' %
(t_heap_world[0], t_heap_world[1]))
# sample object, center of mass, pose
objs_in_heap = []
total_drops = 0
while total_drops < total_num_objs and len(objs_in_heap) < num_objs:
obj_key = sample_keys[total_drops]
obj_mesh = trimesh.load_mesh(self.mesh_filenames[obj_key])
obj_mesh.density = self.obj_density
obj = ObjectState(obj_key, obj_mesh)
_, radius = trimesh.nsphere.minimum_nsphere(obj.mesh)
if 2 * radius > self.max_obj_diam:
self._logger.warning('Obj too big, skipping .....')
total_drops += 1
continue
# sample center of mass
delta_com = self.delta_com_rv.rvs(size=1)
center_of_mass = obj.mesh.center_mass + delta_com
obj.mesh.center_mass = center_of_mass
# sample obj drop pose
obj_orientation = self.obj_orientation_space.sample()
az = obj_orientation[0]
elev = obj_orientation[1]
T_obj_table = RigidTransform.sph_coords_to_pose(az,
elev).as_frames(
'obj', 'world')
# sample object planar pose
obj_planar_pose = self.obj_planar_pose_space.sample()
theta = obj_planar_pose[2]
R_table_world = RigidTransform.z_axis_rotation(theta)
R_obj_drop_world = R_table_world.dot(T_obj_table.rotation)
t_obj_drop_heap = np.array(
[obj_planar_pose[0], obj_planar_pose[1], self.drop_height])
t_obj_drop_world = t_obj_drop_heap + t_heap_world
obj.pose = RigidTransform(rotation=R_obj_drop_world,
translation=t_obj_drop_world,
from_frame='obj',
to_frame='world')
self._physics_engine.add(obj)
try:
v, w = self._physics_engine.get_velocity(obj.key)
except:
self._logger.warning(
'Could not get base velocity for object %s. Skipping ...' %
(obj_key))
self._physics_engine.remove(obj.key)
total_drops += 1
continue
objs_in_heap.append(obj)
# setup until approx zero velocity
wait = time.time()
objects_in_motion = True
num_steps = 0
while objects_in_motion and num_steps < self.max_settle_steps:
# step simulation
self._physics_engine.step()
# check velocities
max_mag_v = 0
max_mag_w = 0
objs_to_remove = set()
for o in objs_in_heap:
try:
v, w = self._physics_engine.get_velocity(o.key)
except:
self._logger.warning(
'Could not get base velocity for object %s. Skipping ...'
% (o.key))
objs_to_remove.add(o)
continue
mag_v = np.linalg.norm(v)
mag_w = np.linalg.norm(w)
if mag_v > max_mag_v:
max_mag_v = mag_v
if mag_w > max_mag_w:
max_mag_w = mag_w
# Remove invalid objects
for o in objs_to_remove:
self._physics_engine.remove(o.key)
objs_in_heap.remove(o)
# check objs in motion
if max_mag_v < self.mag_v_thresh and max_mag_w < self.mag_w_thresh:
objects_in_motion = False
num_steps += 1
# read out object poses
objs_to_remove = set()
for o in objs_in_heap:
obj_pose = self._physics_engine.get_pose(o.key)
o.pose = obj_pose.copy()
# remove the object if its outside of the workspace
if not self.in_workspace(obj_pose):
self._logger.warning(
'Object {} fell out of the workspace!'.format(o.key))
objs_to_remove.add(o)
# remove invalid objects
for o in objs_to_remove:
self._physics_engine.remove(o.key)
objs_in_heap.remove(o)
total_drops += 1
self._logger.debug('Waiting for zero velocity took %.3f sec' %
(time.time() - wait))
# Stop physics engine
self._physics_engine.stop()
# add metadata for heap state and return it
metadata = {'split': TRAIN_ID}
if not train:
metadata['split'] = TEST_ID
return HeapState(workspace_obj_states, objs_in_heap, metadata=metadata)
def register(sensor, config):
"""
Registers a camera to a chessboard.
Parameters
----------
sensor : :obj:`perception.RgbdSensor`
the sensor to register
config : :obj:`autolab_core.YamlConfig` or :obj:`dict`
configuration file for registration
Returns
-------
:obj:`ChessboardRegistrationResult`
the result of registration
Notes
-----
The config must have the parameters specified in the Other Parameters section.
Other Parameters
----------------
num_transform_avg : int
the number of independent registrations to average together
num_images : int
the number of images to read for each independent registration
corners_x : int
the number of chessboard corners in the x-direction
corners_y : int
the number of chessboard corners in the y-direction
color_image_rescale_factor : float
amount to rescale the color image for detection (numbers around 4-8 are useful)
vis : bool
whether or not to visualize the registration
"""
# read config
num_transform_avg = config['num_transform_avg']
num_images = config['num_images']
sx = config['corners_x']
sy = config['corners_y']
point_order = config['point_order']
color_image_rescale_factor = config['color_image_rescale_factor']
flip_normal = config['flip_normal']
y_points_left = False
if 'y_points_left' in config.keys() and sx == sy:
y_points_left = config['y_points_left']
num_images = 1
vis = config['vis']
# read params from sensor
logging.info('Registering camera %s' %(sensor.frame))
ir_intrinsics = sensor.ir_intrinsics
# repeat registration multiple times and average results
R = np.zeros([3,3])
t = np.zeros([3,1])
points_3d_plane = PointCloud(np.zeros([3, sx*sy]), frame=sensor.ir_frame)
k = 0
while k < num_transform_avg:
# average a bunch of depth images together
depth_ims = None
for i in range(num_images):
start = time.time()
small_color_im, new_depth_im, _ = sensor.frames()
end = time.time()
logging.info('Frames Runtime: %.3f' %(end-start))
if depth_ims is None:
depth_ims = np.zeros([new_depth_im.height,
new_depth_im.width,
num_images])
depth_ims[:,:,i] = new_depth_im.data
med_depth_im = np.median(depth_ims, axis=2)
depth_im = DepthImage(med_depth_im, sensor.ir_frame)
# find the corner pixels in an upsampled version of the color image
big_color_im = small_color_im.resize(color_image_rescale_factor)
corner_px = big_color_im.find_chessboard(sx=sx, sy=sy)
if vis:
plt.figure()
plt.imshow(big_color_im.data)
for i in range(sx):
plt.scatter(corner_px[i,0], corner_px[i,1], s=25, c='b')
plt.show()
if corner_px is None:
logging.error('No chessboard detected! Check camera exposure settings')
continue
# convert back to original image
small_corner_px = corner_px / color_image_rescale_factor
if vis:
plt.figure()
plt.imshow(small_color_im.data)
for i in range(sx):
plt.scatter(small_corner_px[i,0], small_corner_px[i,1], s=25, c='b')
plt.axis('off')
plt.show()
# project points into 3D
camera_intr = sensor.ir_intrinsics
points_3d = camera_intr.deproject(depth_im)
# get round chessboard ind
corner_px_round = np.round(small_corner_px).astype(np.uint16)
corner_ind = depth_im.ij_to_linear(corner_px_round[:,0], corner_px_round[:,1])
if corner_ind.shape[0] != sx*sy:
logging.warning('Did not find all corners. Discarding...')
continue
# average 3d points
points_3d_plane = (k * points_3d_plane + points_3d[corner_ind]) / (k + 1)
logging.info('Registration iteration %d of %d' %(k+1, config['num_transform_avg']))
k += 1
# fit a plane to the chessboard corners
X = np.c_[points_3d_plane.x_coords, points_3d_plane.y_coords, np.ones(points_3d_plane.num_points)]
y = points_3d_plane.z_coords
A = X.T.dot(X)
b = X.T.dot(y)
w = np.linalg.inv(A).dot(b)
n = np.array([w[0], w[1], -1])
n = n / np.linalg.norm(n)
if flip_normal:
n = -n
mean_point_plane = points_3d_plane.mean()
# find x-axis of the chessboard coordinates on the fitted plane
T_camera_table = RigidTransform(translation = -points_3d_plane.mean().data,
from_frame=points_3d_plane.frame,
to_frame='table')
points_3d_centered = T_camera_table * points_3d_plane
# get points along y
if point_order == 'row_major':
coord_pos_x = int(math.floor(sx*sy/2.0))
coord_neg_x = int(math.ceil(sx*sy/2.0))
points_pos_x = points_3d_centered[coord_pos_x:]
points_neg_x = points_3d_centered[:coord_neg_x]
x_axis = np.mean(points_pos_x.data, axis=1) - np.mean(points_neg_x.data, axis=1)
x_axis = x_axis - np.vdot(x_axis, n)*n
x_axis = x_axis / np.linalg.norm(x_axis)
y_axis = np.cross(n, x_axis)
else:
coord_pos_y = int(math.floor(sx*(sy-1)/2.0))
coord_neg_y = int(math.ceil(sx*(sy+1)/2.0))
points_pos_y = points_3d_centered[:coord_pos_y]
points_neg_y = points_3d_centered[coord_neg_y:]
y_axis = np.mean(points_pos_y.data, axis=1) - np.mean(points_neg_y.data, axis=1)
y_axis = y_axis - np.vdot(y_axis, n)*n
y_axis = y_axis / np.linalg.norm(y_axis)
x_axis = np.cross(-n, y_axis)
# produce translation and rotation from plane center and chessboard basis
rotation_cb_camera = RigidTransform.rotation_from_axes(x_axis, y_axis, n)
translation_cb_camera = mean_point_plane.data
T_cb_camera = RigidTransform(rotation=rotation_cb_camera,
translation=translation_cb_camera,
from_frame='cb',
to_frame=sensor.frame)
if y_points_left and np.abs(T_cb_camera.y_axis[1]) > 0.1:
if T_cb_camera.x_axis[0] > 0:
T_cb_camera.rotation = T_cb_camera.rotation.dot(RigidTransform.z_axis_rotation(-np.pi/2).T)
else:
T_cb_camera.rotation = T_cb_camera.rotation.dot(RigidTransform.z_axis_rotation(np.pi/2).T)
T_camera_cb = T_cb_camera.inverse()
# optionally display cb corners with detected pose in 3d space
if config['debug']:
# display image with axes overlayed
cb_center_im = camera_intr.project(Point(T_cb_camera.translation, frame=sensor.ir_frame))
cb_x_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.x_axis * config['scale_amt'], frame=sensor.ir_frame))
cb_y_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.y_axis * config['scale_amt'], frame=sensor.ir_frame))
cb_z_im = camera_intr.project(Point(T_cb_camera.translation + T_cb_camera.z_axis * config['scale_amt'], frame=sensor.ir_frame))
x_line = np.array([cb_center_im.data, cb_x_im.data])
y_line = np.array([cb_center_im.data, cb_y_im.data])
z_line = np.array([cb_center_im.data, cb_z_im.data])
plt.figure()
plt.imshow(small_color_im.data)
plt.scatter(cb_center_im.data[0], cb_center_im.data[1])
plt.plot(x_line[:,0], x_line[:,1], c='r', linewidth=3)
plt.plot(y_line[:,0], y_line[:,1], c='g', linewidth=3)
plt.plot(z_line[:,0], z_line[:,1], c='b', linewidth=3)
plt.axis('off')
plt.title('Chessboard frame in camera %s' %(sensor.frame))
plt.show()
return ChessboardRegistrationResult(T_camera_cb, points_3d_plane)
def get_camera_poses(config, frame='world'):
"""Get a list of camera-to-frame poses broken up uniformly about a viewsphere.
Parameters
----------
config : autolab_core.YamlConfig
A configuration containing parameters of the random variable.
frame : str
The name of the target world frame.
Notes
-----
Required parameters of config are specified in Other Parameters.
Other Parameters
----------------
radius: Distance from camera to world origin.
min : float
max : float
n : int
azimuth: Azimuth (angle from x-axis) of camera in degrees.
min : float
max : float
n : int
elevation: Elevation (angle from z-axis) of camera in degrees.
min : float
max : float
n : int
roll: Roll (angle about view direction) of camera in degrees.
min : float
max : float
n : int
Returns
-------
list of autolab_core.RigidTransform
A list of camera-to-frame transforms.
"""
min_radius = config['radius']['min']
max_radius = config['radius']['max']
num_radius = config['radius']['n']
radii = np.linspace(min_radius, max_radius, num_radius)
min_azimuth = config['azimuth']['min']
max_azimuth = config['azimuth']['max']
num_azimuth = config['azimuth']['n']
azimuths = np.linspace(min_azimuth, max_azimuth, num_azimuth)
min_elev = config['elev']['min']
max_elev = config['elev']['max']
num_elev = config['elev']['n']
elevs = np.linspace(min_elev, max_elev, num_elev)
min_roll = config['roll']['min']
max_roll = config['roll']['max']
num_roll = config['roll']['n']
rolls = np.linspace(min_roll, max_roll, num_roll)
camera_to_frame_tfs = []
for r in radii:
for a in azimuths:
for e in elevs:
for roll in rolls:
cam_center = np.array([sph2cart(r, a, e)]).squeeze()
cz = -cam_center / np.linalg.norm(cam_center)
cx = np.array([cz[1], -cz[0], 0])
if np.linalg.norm(cx) == 0:
cx = np.array([1.0, 0.0, 0.0])
cx = cx / np.linalg.norm(cx)
cy = np.cross(cz, cx)
cy = cy / np.linalg.norm(cy)
if cy[2] > 0:
cx = -cx
cy = np.cross(cz, cx)
cy = cy / np.linalg.norm(cy)
R_cam_frame = np.array([cx, cy, cz]).T
R_roll = RigidTransform.z_axis_rotation(roll)
R_cam_frame = R_cam_frame.dot(R_roll)
T_camera_frame = RigidTransform(R_cam_frame, cam_center,
from_frame='camera', to_frame=frame)
camera_to_frame_tfs.append(T_camera_frame)
return camera_to_frame_tfs
from frankapy.proto import PosePositionSensorMessage, ShouldTerminateSensorMessage, CartesianImpedanceSensorMessage
from franka_interface_msgs.msg import SensorDataGroup
from frankapy.utils import min_jerk, min_jerk_weight
import rospy
if __name__ == "__main__":
fa = FrankaArm()
fa.reset_joints()
rospy.loginfo('Generating Trajectory')
p0 = fa.get_pose()
p1 = p0.copy()
T_delta = RigidTransform(translation=np.array([0, 0, 0.2]),
rotation=RigidTransform.z_axis_rotation(
np.deg2rad(30)),
from_frame=p1.from_frame,
to_frame=p1.from_frame)
p1 = p1 * T_delta
fa.goto_pose(p1)
T = 5
dt = 0.02
ts = np.arange(0, T, dt)
weights = [min_jerk_weight(t, T) for t in ts]
pose_traj = [p1.interpolate_with(p0, w) for w in weights]
z_stiffness_traj = [min_jerk(100, 800, t, T) for t in ts]
rospy.loginfo('Initializing Sensor Publisher')
