def gripper_on_object(gripper,
grasp,
obj,
stable_pose=None,
T_table_world=RigidTransform(from_frame='table',
to_frame='world'),
gripper_color=(0.5, 0.5, 0.5),
object_color=(0.5, 0.5, 0.5),
style='surface',
plot_table=True,
table_dim=0.15):
""" Visualize a gripper on an object.
Parameters
----------
gripper : :obj:`dexnet.grasping.RobotGripper`
gripper to plot
grasp : :obj:`dexnet.grasping.Grasp`
grasp to plot the gripper in
obj : :obj:`dexnet.grasping.GraspableObject3D`
3D object to plot the gripper on
stable_pose : :obj:`meshpy.StablePose` or :obj:`autolab_core.RigidTransform`
stable pose of the object on a planar worksurface
T_table_world : :obj:`autolab_core.RigidTransform`
pose of table, specified as a transformation from mesh frame to world frame
gripper_color : 3-tuple
color of the gripper mesh
object_color : 3-tuple
color of the object mesh
style : :obj:`str`
color of the object mesh
plot_table : bool
whether or not to plot the table
table_dim : float
dimension of the table
"""
if stable_pose is None:
Visualizer3D.mesh(obj.mesh, color=object_color, style=style)
T_obj_world = RigidTransform(from_frame='obj', to_frame='world')
elif isinstance(stable_pose, StablePose):
T_obj_world = Visualizer3D.mesh_stable_pose(
obj.mesh,
stable_pose,
T_table_world=T_table_world,
color=object_color,
style=style,
plot_table=plot_table,
dim=table_dim)
else:
T_obj_world = Visualizer3D.mesh_table(obj.mesh,
stable_pose,
T_table_world=T_table_world,
color=object_color,
style=style,
plot_table=plot_table,
dim=table_dim)
DexNetVisualizer3D.gripper(gripper,
grasp,
T_obj_world,
color=gripper_color)
def test_composition(self):
R_a_b = RigidTransform.random_rotation()
t_a_b = RigidTransform.random_translation()
R_b_c = RigidTransform.random_rotation()
t_b_c = RigidTransform.random_translation()
T_a_b = RigidTransform(R_a_b, t_a_b, "a", "b")
T_b_c = RigidTransform(R_b_c, t_b_c, "b", "c")
# multiply with numpy arrays
M_a_b = np.r_[np.c_[R_a_b, t_a_b], [[0, 0, 0, 1]]]
M_b_c = np.r_[np.c_[R_b_c, t_b_c], [[0, 0, 0, 1]]]
M_a_c = M_b_c.dot(M_a_b)
# use multiplication operator
T_a_c = T_b_c * T_a_b
self.assertTrue(
np.sum(np.abs(T_a_c.matrix - M_a_c)) < 1e-5,
msg="Composition gave incorrect transformation",
)
# check frames
self.assertEqual(
T_a_c.from_frame, "a", msg="Composition has incorrect input frame"
)
self.assertEqual(
T_a_c.to_frame, "c", msg="Composition has incorrect output frame"
)
def visualize_scene(mesh, T_world_ar, T_ar_cam, T_cam_obj):
T_cam_cam = RigidTransform()
T_obj_cam = T_cam_obj.inverse()
T_cam_ar = T_ar_cam.inverse()
Twc = np.matmul(T_world_ar.matrix, T_ar_cam.matrix)
T_world_cam = RigidTransform(Twc[:3, :3], Twc[:3, 3])
T_cam_world = T_world_cam.inverse()
o = np.array([0., 0., 0.])
centroid_cam = apply_transform(o, T_cam_obj)
tag_cam = apply_transform(o, T_cam_ar)
robot_cam = apply_transform(o, T_cam_world)
# camera
vis3d.points(o, color=(0, 1, 0), scale=0.01)
vis3d.pose(T_cam_cam, alpha=0.05, tube_radius=0.005, center_scale=0.002)
# object
vis3d.mesh(mesh)
vis3d.points(centroid_cam, color=(0, 0, 0), scale=0.01)
vis3d.pose(T_cam_obj, alpha=0.05, tube_radius=0.005, center_scale=0.002)
# AR tag
vis3d.points(tag_cam, color=(1, 0, 1), scale=0.01)
vis3d.pose(T_cam_ar, alpha=0.05, tube_radius=0.005, center_scale=0.002)
vis3d.table(T_cam_ar, dim=0.074)
# robot
vis3d.points(robot_cam, color=(1, 0, 0), scale=0.01)
vis3d.pose(T_cam_world, alpha=0.05, tube_radius=0.005, center_scale=0.002)
vis3d.show()
def lookup_transform(to_frame, from_frame='base'):
"""
Returns the AR tag position in world coordinates
Parameters
----------
to_frame : string
examples are: ar_marker_7, gearbox, pawn, ar_marker_3, etc
from_frame : string
lets be real, you're probably only going to use 'base'
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position or object in world coordinates
"""
if not ros_enabled:
print 'I am the lookup transform function! ' \
+ 'You\'re not using ROS, so I\'m returning the Identity Matrix.'
return RigidTransform(to_frame=from_frame, from_frame=to_frame)
listener = tf.TransformListener()
attempts, max_attempts, rate = 0, 10, rospy.Rate(1.0)
while attempts < max_attempts:
try:
t = listener.getLatestCommonTime(from_frame, to_frame)
tag_pos, tag_rot = listener.lookupTransform(from_frame, to_frame, t)
except:
rate.sleep()
attempts += 1
rot = RigidTransform.rotation_from_quaternion(tag_rot)
return RigidTransform(rot, tag_pos, to_frame=from_frame, from_frame=to_frame)
def render_depth_maps(mesh_filename, intrinsic, extrinsics, mesh_scale=1.0):
scene = mr.Scene()
# setup camera
ci = CameraIntrinsics(frame='camera',
fx=intrinsic.get_focal_length()[0],
fy=intrinsic.get_focal_length()[1],
cx=intrinsic.get_principal_point()[0],
cy=intrinsic.get_principal_point()[1],
skew=0,
height=intrinsic.height,
width=intrinsic.width)
cp = RigidTransform(from_frame='camera', to_frame='world')
camera = mr.VirtualCamera(ci, cp)
scene.camera = camera
obj = trimesh.load_mesh(mesh_filename)
obj.apply_scale(mesh_scale)
dmaps = []
for T in extrinsics:
obj_pose = RigidTransform(rotation=T[:3, :3],
translation=T[:3, 3],
from_frame='obj',
to_frame='camera')
sobj = mr.SceneObject(obj, obj_pose)
scene.add_object('obj', sobj)
dmap = scene.render(render_color=False)
dmap = np.uint16(dmap * 1000.0)
dmaps.append(dmap)
scene.remove_object('obj')
return np.stack(dmaps)
def scroll(self, clicks):
"""Zoom using a mouse scroll wheel motion.
Parameters
----------
clicks : int
The number of clicks. Positive numbers indicate forward wheel movement.
"""
target = self._target
ratio = 0.90
z_axis = self._n_T_camera_world.matrix[:3, 2].flatten()
eye = self._n_T_camera_world.matrix[:3, 3].flatten()
radius = np.linalg.norm(eye - target)
translation = clicks * (1 - ratio) * radius * z_axis
t_tf = RigidTransform(translation=translation,
from_frame='world',
to_frame='world')
self._n_T_camera_world = t_tf.dot(self._n_T_camera_world)
z_axis = self._T_camera_world.matrix[:3, 2].flatten()
eye = self._T_camera_world.matrix[:3, 3].flatten()
radius = np.linalg.norm(eye - target)
translation = clicks * (1 - ratio) * radius * z_axis
t_tf = RigidTransform(translation=translation,
from_frame='world',
to_frame='world')
self._T_camera_world = t_tf.dot(self._T_camera_world)
def rotate(self, azimuth, axis=None):
"""Rotate the trackball about the "Up" axis by azimuth radians.
Parameters
----------
azimuth : float
The number of radians to rotate.
"""
target = self._target
y_axis = self._n_T_camera_world.matrix[:3, 1].flatten()
if axis is not None:
y_axis = axis
x_rot_mat = transformations.rotation_matrix(-azimuth, y_axis, target)
x_rot_tf = RigidTransform(x_rot_mat[:3, :3],
x_rot_mat[:3, 3],
from_frame='world',
to_frame='world')
self._n_T_camera_world = x_rot_tf.dot(self._n_T_camera_world)
y_axis = self._T_camera_world.matrix[:3, 1].flatten()
if axis is not None:
y_axis = axis
x_rot_mat = transformations.rotation_matrix(-azimuth, y_axis, target)
x_rot_tf = RigidTransform(x_rot_mat[:3, :3],
x_rot_mat[:3, 3],
from_frame='world',
to_frame='world')
self._T_camera_world = x_rot_tf.dot(self._T_camera_world)
def lookup_tag(tag_number):
""" Returns the AR tag position in world coordinates
Parameters
----------
tag_number : int
AR tag number
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position in world coordinates
"""
to_frame = 'ar_marker_{}'.format(tag_number)
tag_pos = None
while not tag_pos:
try:
t = listener.getLatestCommonTime(from_frame, to_frame)
# print(rospy.Time.now())
# t = rospy.Time.now()
tag_pos, tag_rot = listener.lookupTransform(from_frame, to_frame, t)
except:
continue;
# if not listener.frameExists(from_frame) or not listener.frameExists(to_frame):
# print 'Frames not found'
# print 'Did you place AR marker {} within view of the baxter left hand camera?'.format(tag_number)
# exit(0)
angles = tfs.euler_from_quaternion(tag_rot)
# print(angles)
trans = tfs.euler_matrix(*angles)
print(RigidTransform(trans[:3, :3], tag_pos))
return RigidTransform(trans[:3, :3], tag_pos)
def predict(self):
rot, trans = RigidTransform.rotation_and_translation_from_matrix(
self.datapoint['obj_pose'])
dataset_pose = RigidTransform(rot, trans, 'obj', 'world')
dataset_final_poses = self.datapoint['final_poses']
num_vertices = self.datapoint['num_vertices']
final_poses = []
for i in range(20):
final_pose_mat = dataset_final_poses[i * 4:(i + 1) * 4]
if np.array_equal(final_pose_mat, np.zeros((4, 4))):
break
rot, trans = RigidTransform.rotation_and_translation_from_matrix(
final_pose_mat)
final_poses.append(RigidTransform(rot, trans, 'obj', 'world'))
vertices = self.datapoint['vertices'][:num_vertices]
normals = self.datapoint['normals'][:num_vertices]
vertices = (self.obj.T_obj_world * dataset_pose.inverse() *
PointCloud(vertices.T, 'world')).data.T
normals = (self.obj.T_obj_world * dataset_pose.inverse() *
PointCloud(normals.T, 'world')).data.T
vertex_probs = self.datapoint[
'vertex_probs'][:num_vertices, :len(final_poses)]
required_forces = self.datapoint['min_required_forces'][:num_vertices]
return vertices, normals, final_poses, vertex_probs, required_forces
def generate_images(self, salient_edge_set, n_samples=1):
"""Generate depth image, normal image, and binary edge mask tuples.
Parameters
----------
salient_edge_set : SalientEdgeSet
A salient edge set to generate images of.
n_samples : int
The number of samples to generate.
Returns
-------
depth_ims : (n,) list of perception.DepthImage
Randomly-rendered depth images of object.
normal_ims : (n,) list of perception.PointCloudImage
Normals for the given image
edge_masks : (n,) list of perception.BinaryImage
Masks for pixels on the salient edges of the object.
"""
# Compute stable poses of mesh
mesh = salient_edge_set.mesh
stp_pose_tfs, probs = mesh.compute_stable_poses()
probs = probs / sum(probs)
# Generate n renders
depth_ims, normal_ims, edge_masks = [], [], []
scene = Scene()
so = SceneObject(mesh, RigidTransform(from_frame='obj', to_frame='world'))
scene.add_object('object', so)
for i in range(n_samples):
# Sample random stable pose.
tf_id = np.random.choice(np.arange(len(probs)), p=probs)
tf = stp_pose_tfs[tf_id]
T_obj_world = RigidTransform(tf[:3,:3], tf[:3,3], from_frame='obj', to_frame='world')
so.T_obj_world = T_obj_world
# Create the random uniform workspace sampler
ws_cfg = self._config['worksurface_rv_config']
uvs = UniformPlanarWorksurfaceImageRandomVariable('object', scene, [RenderMode.DEPTH], frame='camera', config=ws_cfg)
# Sample and extract the depth image, camera intrinsics, and T_obj_camera
sample = uvs.sample()
depth_im = sample.renders[RenderMode.DEPTH]
cs = sample.camera
ci = CameraIntrinsics(frame='camera', fx=cs.focal, fy=cs.focal, cx=cs.cx, cy=cs.cy,
skew=0.0, height=ws_cfg['im_height'], width=ws_cfg['im_width'])
T_obj_camera = cs.T_camera_world.inverse().dot(T_obj_world)
edge_mask = self._compute_edge_mask(salient_edge_set, depth_im, ci, T_obj_camera)
point_cloud_im = ci.deproject_to_image(depth_im)
normal_im = point_cloud_im.normal_cloud_im()
depth_ims.append(depth_im)
normal_ims.append(normal_im)
edge_masks.append(edge_mask)
return depth_ims, normal_ims, edge_masks
def figure_0():
action = policy.action(env.state)
env.render_3d_scene()
bottom_points = policy.toppling_model.bottom_points
vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.001)
mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
mesh.fix_normals()
direction = normalize([0, -.04, 0])
origin = mesh.center_mass + np.array([0, .04, .09])
intersect, _, face_ind = mesh.ray.intersects_location([origin],
[direction],
multiple_hits=False)
normal = mesh.face_normals[face_ind[0]]
start_point = intersect[0] + .06 * normal
end_point = intersect[0]
shaft_points = [start_point, end_point]
h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071,
0.7071]]).dot(-normal)
h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071,
0.7071]]).dot(-normal)
head_points = [end_point - 0.02 * h2, end_point, end_point - 0.02 * h1]
vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.002)
vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.002)
vis3d.points(Point(end_point), scale=.004, color=[0, 0, 0])
hand_pose = RigidTransform(rotation=policy.get_hand_pose(
start_point, end_point)[0].rotation,
translation=start_point,
from_frame='grasp',
to_frame='world')
orig_pose = env.state.obj.T_obj_world.copy()
env.state.obj.T_obj_world = policy.toppling_model.final_poses[1]
action = policy.grasping_policy.action(env.state)
env.state.obj.T_obj_world = orig_pose
gripper = env.gripper(action)
#vis3d.mesh(gripper.mesh, hand_pose * gripper.T_mesh_grasp, color=light_blue)
# mesh = trimesh.load('~/Downloads/chris.stl')
rot = np.array([[0, -1, 0], [1, 0, 0],
[0, 0,
1]]).dot(np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]))
T = RigidTransform(rotation=rot,
translation=np.array([0, -.09, .1]),
to_frame='mesh',
from_frame='mesh')
# vis3d.mesh(mesh, hand_pose * gripper.T_mesh_grasp * T, color=light_blue)
vis3d.mesh(gripper.mesh,
hand_pose * gripper.T_mesh_grasp * T,
color=light_blue)
vis3d.show(starting_camera_pose=CAMERA_POSE)
env.state.obj.T_obj_world = policy.toppling_model.final_poses[1]
action = policy.grasping_policy.action(env.state)
print 'q:', action.q_value
env.render_3d_scene()
vis3d.gripper(env.gripper(action),
action.grasp(env.gripper(action)),
color=light_blue)
vis3d.show(starting_camera_pose=CAMERA_POSE)
def processing(self):
"""
Transformation: Link8 -> Grasp -> Camera -> Link0
"""
# Transforms using Listener
listener = tf.TransformListener()
while True:
try:
(tcr, rcr) = listener.lookupTransform('/panda_link0', '/color',
rospy.Time(0))
(tref,
rref) = listener.lookupTransform('/panda_link0',
'/panda_link8',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
break
# Ready Position
qref = [rref[3], rref[0], rref[1], rref[2]]
self.ready_pose = RigidTransform(rotation=qref,
translation=tref,
from_frame='link8',
to_frame='link0').pose_msg
# Transformation from Camera to Robot link 0 (base) Frame
qcr = [rcr[3], rcr[0], rcr[1], rcr[2]]
T_camera_robot = RigidTransform(rotation=qcr,
translation=tcr,
from_frame='camera',
to_frame='link0')
# Transformation from Approach Grasp Frame to Camera Frame
T_gripper_camera = RigidTransform(rotation=self.orientation,
translation=self.position,
from_frame='grasp',
to_frame='camera')
# Transformation from Grasp Frame to Camera Frame
T_gripper_camera1 = RigidTransform(rotation=self.orientation,
translation=self.position1,
from_frame='grasp',
to_frame='camera')
# Transformation from End-Effector/Tool Frame (Panda Link 8) to Grasp Frame
### Rotation: YZX = [90,45,0]
eef_rot = [0.6532815, 0.2705981, 0.6532815, 0.2705981]
eef_pos = [-0.050, -0.030, -0.01]
T_eef_gripper = RigidTransform(rotation=eef_rot,
translation=eef_pos,
from_frame='eef',
to_frame='grasp')
# Transformation from End-Effector/Tool Frame (Panda Link 8) to Robot Base Frame
# Approach
self.T_gripper_robot = T_camera_robot * T_gripper_camera * T_eef_gripper
# Grasp
self.T_gripper_robot1 = T_camera_robot * T_gripper_camera1 * T_eef_gripper
def plate_push_catch(self):
self.right.goto_pose(
RigidTransform(translation=[0.3, 0, 0.2],
rotation=right_front_rotation))
self.right.goto_pose(
RigidTransform(translation=[0.3, 0, 0.07],
rotation=right_front_rotation))
time.sleep(3)
def plate_slide(self):
self.right.goto_pose(
RigidTransform(translation=[0.72, 0, 0.35],
rotation=right_front_rotation))
time.sleep(3)
self.right.goto_pose(
RigidTransform(translation=[0.75, 0, 0.35],
rotation=right_front_give_rotation))
def plate_drop(self):
self.right.goto_pose(
RigidTransform(translation=[0.72, 0, 0.35],
rotation=right_front_rotation))
time.sleep(3)
self.right.open_gripper()
self.right.goto_pose(
RigidTransform(translation=[0.4, 0, 0.3],
rotation=right_front_rotation))
def lookup_transform(to_frame, from_frame='base'):
"""
Returns the AR tag position in world coordinates
Parameters
----------
to_frame : string
examples are: ar_marker_7, gearbox, pawn, ar_marker_3, etc
from_frame : string
lets be real, you're probably only going to use 'base'
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position or object in world coordinates
"""
if not ros_enabled:
print 'I am the lookup transform function! ' \
+ 'You\'re not using ROS, so I\'m returning the Identity Matrix.'
return RigidTransform(to_frame=from_frame, from_frame=to_frame)
listener = tf.TransformListener()
attempts, max_attempts, rate = 0, 10, rospy.Rate(1.0)
while attempts < max_attempts:
try:
# print("in try")
# print("1 here")
t = listener.getLatestCommonTime(from_frame, to_frame)
# print("here")
tag_pos, tag_rot = listener.lookupTransform(
from_frame, to_frame, t)
real_rot = np.array(
[tag_rot[3], tag_rot[0], tag_rot[1], tag_rot[2]])
# real_rot = np.array([tag_rot[1], tag_rot[2], tag_rot[3], tag_rot[0]])
break
except:
rate.sleep()
attempts += 1
# print("t",t)
# print("tag_pos", tag_pos)
# # print("tag_rot", tag_rot)
# print("real_rot", real_rot)
### comment out when using ar tag
# tag_rot = [0, 0, 1, 0]
# tag_pos = [0.570, -0.184, -.243] #changed this so it would work for right gripper better
### comment out when using ar tag
rot = RigidTransform.rotation_from_quaternion(real_rot)
# rot = tag_rot
# print(from_frame, to_frame)
return RigidTransform(rot,
tag_pos,
to_frame=from_frame,
from_frame=to_frame)
def test_bad_inits(self):
# test bad rotation dim
R = np.random.rand(3)
caught_bad_rotation = False
try:
RigidTransform(R)
except ValueError:
caught_bad_rotation = True
self.assertTrue(
caught_bad_rotation, msg="Failed to catch 3x1 rotation matrix"
)
# test bad rotation dim
R = np.random.rand(3, 3, 3)
caught_bad_rotation = False
try:
RigidTransform(R)
except ValueError:
caught_bad_rotation = True
self.assertTrue(
caught_bad_rotation, msg="Failed to catch 3x3x3 rotation matrix"
)
# determinant not equal to one
R = np.random.rand(3, 3)
caught_bad_rotation = False
try:
RigidTransform(R)
except ValueError:
caught_bad_rotation = True
self.assertTrue(
caught_bad_rotation, msg="Failed to catch rotation with det != 1"
)
# translation with illegal dimensions
t = np.random.rand(3, 3)
caught_bad_translation = False
try:
RigidTransform(translation=t)
except ValueError:
caught_bad_translation = True
self.assertTrue(
caught_bad_translation, msg="Failed to catch 3x3 translation"
)
# translation with illegal dimensions
t = np.random.rand(2)
caught_bad_translation = False
try:
RigidTransform(translation=t)
except ValueError:
caught_bad_translation = True
self.assertTrue(
caught_bad_translation, msg="Failed to catch 2x1 translation"
)
def vertices_to_baxter_hand_pose(self,
grasp_vertices,
approach_direction,
Rot,
obj_name='gearbox'):
"""
takes the contacts positions in the object frame and returns the hand pose T_obj_gripper
BE CAREFUL ABOUT THE FROM FRAME AND TO FRAME. the RigidTransform class' frames are
weird.
Parameters
----------
grasp_vertices : 2x3 :obj:`numpy.ndarray`
position of the fingers in object frame
approach_direction : 3x' :obj:`numpy.ndarray`
there are multiple grasps that go through contact1 and contact2. This describes which
orientation the hand should be in
Returns
-------
:obj:`autolab_core:RigidTransform` Hand pose in the object frame Note: in object frame
"""
# YOUR CODE HERE
v1, v2 = grasp_vertices
center = (v1 + v2) / 2
#print("v1,v2,center", v1, v2, center)
#print("App_dir:", approach_direction)
rot = look_at_general(center, approach_direction)
wrist_position = center #- approach_direction*GRIPPER_LENGTH # note that this only works b/c I made sure approach_direction is normalized
# print("wrist_position ", wrist_position)
print("approach direction: ", approach_direction)
print("vertices", v1, v2)
# time.sleep(10)
# wrist_position assumes is the position of the wrist (duh) and not the grippers
#print("rotation", rot)
# rot = rot[:3, :3]
modified_identity = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]])
rot = np.dot(modified_identity, np.asarray(Rot[:3, :3]))
rpy = tf.transformations.euler_from_matrix(rot)
print("!!!!!!!!!!RPY!!!!!!!!!!")
print("rot in rpy", rpy)
g = RigidTransform(rot,
wrist_position,
from_frame='grasp',
to_frame=obj_name)
print(" ## g ##### ", g)
# I quite not sure here
return RigidTransform(rot,
wrist_position,
from_frame='grasp',
to_frame=obj_name)
def test_linear_trajectory(self):
R_a = RigidTransform.random_rotation()
t_a = RigidTransform.random_translation()
R_b = RigidTransform.random_rotation()
t_b = RigidTransform.random_translation()
T_a = RigidTransform(R_a, t_a, "w", "a")
T_b = RigidTransform(R_b, t_b, "w", "b")
for i in range(10):
traj = T_a.linear_trajectory_to(T_b, i)
self.assertEqual(len(traj), i, "Trajectory has incorrect length")
def test_registration(self):
np.random.seed(101)
source_points = np.random.rand(3, NUM_POINTS).astype(np.float32)
source_normals = np.random.rand(3, NUM_POINTS).astype(np.float32)
source_normals = source_normals / np.tile(
np.linalg.norm(source_normals, axis=0)[np.newaxis, :], [3, 1])
source_point_cloud = PointCloud(source_points, frame='world')
source_normal_cloud = NormalCloud(source_normals, frame='world')
matcher = PointToPlaneFeatureMatcher()
solver = PointToPlaneICPSolver(sample_size=NUM_POINTS)
# 3d registration
tf = RigidTransform(rotation=RigidTransform.random_rotation(),
translation=RigidTransform.random_translation(),
from_frame='world',
to_frame='world')
tf = RigidTransform(from_frame='world',
to_frame='world').interpolate_with(tf, 0.01)
target_point_cloud = tf * source_point_cloud
target_normal_cloud = tf * source_normal_cloud
result = solver.register(source_point_cloud,
target_point_cloud,
source_normal_cloud,
target_normal_cloud,
matcher,
num_iterations=NUM_ITERS)
self.assertTrue(
np.allclose(tf.matrix, result.T_source_target.matrix, atol=1e-3))
# 2d registration
theta = 0.1 * np.random.rand()
t = 0.005 * np.random.rand(3, 1)
t[2] = 0
R = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0], [0, 0, 1]])
tf = RigidTransform(R, t, from_frame='world', to_frame='world')
target_point_cloud = tf * source_point_cloud
target_normal_cloud = tf * source_normal_cloud
result = solver.register_2d(source_point_cloud,
target_point_cloud,
source_normal_cloud,
target_normal_cloud,
matcher,
num_iterations=NUM_ITERS)
self.assertTrue(
np.allclose(tf.matrix, result.T_source_target.matrix, atol=1e-3))
def plate_slide_2(self):
# self.right.goto_pose(RigidTransform(translation = [0.72, 0, 0.35], rotation = right_front_rotation))
self.right.goto_pose(
RigidTransform(translation=[0.3, 0, 0.25],
rotation=right_front_rotation))
self.right.goto_pose(
RigidTransform(translation=[0.3, 0, 0.07],
rotation=right_front_rotation))
time.sleep(3)
self.right.open_gripper()
self.right.goto_pose(
RigidTransform(translation=[0.33, 0, 0.2],
rotation=right_front_rotation))
def lookup_transform(to_frame, from_frame='base'):
"""
Returns the AR tag position in world coordinates
Parameters
----------
to_frame : string
examples are: ar_marker_7, gearbox, pawn, ar_marker_3, etc
from_frame : string
lets be real, you're probably only going to use 'base'
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position or object in world coordinates
"""
if not ros_enabled:
print 'I am the lookup transform function! ' \
+ 'You\'re not using ROS, so I\'m returning the Identity Matrix.'
return RigidTransform(to_frame=from_frame, from_frame=to_frame)
listener = tf.TransformListener()
attempts, max_attempts, rate = 0, 10, rospy.Rate(1.0)
# Sleep for a bit before looking up transformation
for _ in range(1):
rate.sleep()
while attempts < max_attempts:
try:
# import pdb; pdb.set_trace()
t = listener.getLatestCommonTime(from_frame, to_frame)
# original
# tag_pos, tag_rot = listener.lookupTransform(from_frame, to_frame, t)
# changes from piazza
tag_pos, tag_rot = listener.lookupTransform(
to_frame, from_frame, t)
tag_rot = np.roll(tag_rot, 1)
break
except:
rate.sleep()
attempts += 1
print 'Attempt {0} to look up transformation from {1} to {2} unsuccessful.'.format(
attempts, from_frame, to_frame)
print 'Successfully found transformation from {0} to {1}.'.format(
from_frame, to_frame)
# import pdb; pdb.set_trace()
rot = RigidTransform.rotation_from_quaternion(tag_rot)
# original
return RigidTransform(rot,
tag_pos,
to_frame=from_frame,
from_frame=to_frame)
def straighten_transform(rt):
angles = rt.euler_angles
roll_off = angles[0]
pitch_off = angles[1]
roll_fix = RigidTransform(
rotation=RigidTransform.x_axis_rotation(np.pi - roll_off),
from_frame=rt.from_frame,
to_frame=rt.from_frame)
pitch_fix = RigidTransform(
rotation=RigidTransform.y_axis_rotation(pitch_off),
from_frame=rt.from_frame,
to_frame=rt.from_frame)
new_rt = rt * roll_fix * pitch_fix
return new_rt
def lookup_transform(to_frame, baxter, from_frame='base'):
"""
Returns the AR tag position in world coordinates
Parameters
----------
to_frame : string
examples are: ar_marker_7, gearbox, pawn, ar_marker_3, etc
from_frame : string
lets be real, you're probably only going to use 'base'
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position or object in world coordinates
"""
# if to_frame =='pawn':
# to_frame = 'ar_marker_9'
if not baxter:
print( 'I am the lookup transform function! ' \
+ 'You\'re not using ROS, so I\'m returning the Identity Matrix.')
tag_pos = [0.642, -0.132, -0.023]
tag_rot = [-0.001, -0.030, 0.024, 0.999]
rot = RigidTransform.rotation_from_quaternion(tag_rot)
return RigidTransform(rot,
tag_pos,
to_frame=from_frame,
from_frame=to_frame)
#return RigidTransform(translation = tag_pos, to_frame=from_frame, from_frame=to_frame)
listener = tf.TransformListener()
attempts, max_attempts, rate = 0, 10, rospy.Rate(1.0)
while attempts < max_attempts:
try:
t = listener.getLatestCommonTime(from_frame, to_frame)
tag_pos, tag_rot = listener.lookupTransform(
from_frame, to_frame, t)
break
except:
rate.sleep()
attempts += 1
# tag_pos = [0.642, -0.132, -0.023]
# tag_rot = [-0.001, -0.030, 0.024, 0.999]
rot = RigidTransform.rotation_from_quaternion(tag_rot)
return RigidTransform(rot,
tag_pos,
to_frame=from_frame,
from_frame=to_frame)
def grasp_angles_from_stp_z(self, stable_pose):
""" Get angles of the the grasp from the table plane:
1) the angle between the grasp axis and table normal
2) the angle between the grasp approach axis and the table normal
Parameters
----------
stable_pose : :obj:`StablePose` or :obj:`RigidTransform`
the stable pose to compute the angles for
Returns
-------
psi : float
grasp y axis rotation from z axis in stable pose
phi : float
grasp x axis rotation from z axis in stable pose
"""
T_grasp_obj = self.T_grasp_obj
if isinstance(stable_pose, StablePose):
R_stp_obj = stable_pose.r
else:
R_stp_obj = stable_pose.rotation
T_stp_obj = RigidTransform(R_stp_obj, from_frame='obj', to_frame='stp')
T_stp_grasp = T_stp_obj * T_grasp_obj
stp_z = np.array([0, 0, 1])
grasp_axis_angle = np.arccos(stp_z.dot(T_stp_grasp.y_axis))
grasp_approach_angle = np.arccos(abs(stp_z.dot(T_stp_grasp.x_axis)))
nu = stp_z.dot(T_stp_grasp.z_axis)
return grasp_axis_angle, grasp_approach_angle, nu
def __init__(self,
name,
ar_marker,
R_ar_obj=np.eye(3),
t_ar_obj=np.zeros(3)):
"""
Struct for specifying object templates
Parameters
----------
name : string
name of object
ar_marker : string
name of ar marker on object template
R_ar_obj : 3x3 :obj:`numpy.ndarray`
rotation between AR marker and object
t_ar_obj : 3x' :obj:`numpy.ndarray`
translation between AR marker and objectSS
"""
self.name = name
self.ar_marker = ar_marker
self.T_ar_obj = RigidTransform(rotation=R_ar_obj,
translation=t_ar_obj,
from_frame=name,
to_frame=ar_marker)
def lookup_tag(tag_number):
""" Returns the AR tag position in world coordinates
Parameters
----------
tag_number : int
AR tag number
Returns
-------
:obj:`autolab_core.RigidTransform` AR tag position in world coordinates
"""
listener = tf.TransformListener()
to_frame = 'ar_marker_{}'.format(tag_number)
from_frame = 'base'
while not rospy.is_shutdown():
try:
# if not listener.frameExists(from_frame) or not listener.frameExists(to_frame):
# print 'Frames not found'
# print 'Did you place AR marker {} within view of the baxter left hand camera?'.format(tag_number)
# exit(0)
t = listener.getLatestCommonTime(from_frame, to_frame)
tag_pos, tag_rot = listener.lookupTransform(
from_frame, to_frame, t)
break
except:
continue
tag_rot = np.array(tfs.quaternion_matrix(tag_rot)[0:3, 0:3])
return RigidTransform(tag_rot, tag_pos)
def contacts_to_baxter_hand_pose(contact1, contact2, approach_direction):
""" takes the contacts positions in the object frame and returns the hand pose T_obj_gripper
Parameters
----------
contact1 : :obj:`numpy.ndarray`
position of finger1 in object frame
contact2 : :obj:`numpy.ndarray`
position of finger2 in object frame
approach_direction : :obj:`numpy.ndarray`
there are multiple grasps that go through contact1 and contact2. This describes which
orientation the hand should be in
Returns
-------
:obj:`autolab_core:RigidTransform` Hand pose in the object frame
"""
# translation = average of contact points
translation = (contact1 + contact2) / 2
# x axis = vector pointing between contact points
x_axis = contact1 - contact2
# approach direction = z axis = always vertical... some grasps will fail, that's just how it is
# Chris recommended hardcoding the approach direction like this
z_axis = approach_direction
# find g transform from object frame to grasp frame...
transform = utils.look_at_general(translation, x_axis, z_axis)
print(transform)
T_obj_gripper = RigidTransform(transform[0:3, 0:3], transform[0:3, 3])
return T_obj_gripper
def __init__(self,
mesh,
poses,
T_obj_world=RigidTransform(from_frame='obj',
to_frame='world'),
material=MaterialProperties(),
enabled=True):
"""Initialize a scene object with the given mesh, pose, and material.
Parameters
----------
mesh : trimesh.Trimesh
A mesh representing the object's geometry.
poses : list of autolab_core.RigidTransform
A set of poses, one for each instance of the scene object,
relative to the full object's origin.
T_obj_world : autolab_core.RigidTransform
A rigid transformation from the object's frame to the world frame.
material : MaterialProperties
A set of material properties for the object.
enabled : bool
If False, the object will not be rendered.
"""
super(InstancedSceneObject, self).__init__(mesh, T_obj_world, material,
enabled)
self._poses = poses
def __init__(self,
mesh,
T_obj_world=RigidTransform(from_frame='obj',
to_frame='world'),
material=MaterialProperties(),
enabled=True):
"""Initialize a scene object with the given mesh, pose, and material.
Parameters
----------
mesh : trimesh.Trimesh
A mesh representing the object's geometry.
T_obj_world : autolab_core.RigidTransform
A rigid transformation from the object's frame to the world frame.
material : MaterialProperties
A set of material properties for the object.
enabled : bool
If False, the object will not be rendered.
"""
if not isinstance(mesh, Trimesh):
raise ValueError('mesh must be an object of type Trimesh')
if not isinstance(material, MaterialProperties):
raise ValueError(
'material must be an object of type MaterialProperties')
if material.smooth:
mesh = mesh.smoothed()
self._mesh = mesh
self._material = material
self.T_obj_world = T_obj_world
self._enabled = True
