diff --git a/scripts/hw_node.py b/scripts/hw_node.py index d4c7d2c..d19e28f 100755 --- a/scripts/hw_node.py +++ b/scripts/hw_node.py @@ -81,7 +81,7 @@ class HwNode: def publish_table_co(self, event): msg = geometry_msgs.msg.PoseStamped() msg.header.frame_id = "panda_link0" - msg.pose = to_pose_msg(self.T_base_roi * Transform.t([0.15, 0.15, 0.005])) + msg.pose = to_pose_msg(self.T_base_roi * Transform.t_[0.15, 0.15, 0.005]) self.moveit.scene.add_box("table", msg, size=(0.8, 0.8, 0.01)) diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py index 8ff95b6..3205f9c 100644 --- a/src/active_grasp/controller.py +++ b/src/active_grasp/controller.py @@ -140,7 +140,7 @@ class GraspController: self.create_collision_scene() T_base_grasp = self.postprocess(grasp.pose) self.gripper.move(0.08) - T_base_approach = T_base_grasp * Transform.t([0, 0, -0.06]) * self.T_grasp_ee + T_base_approach = T_base_grasp * Transform.t_[0, 0, -0.06] * self.T_grasp_ee success, plan = self.moveit.plan(T_base_approach, 0.2, 0.2) if success: self.moveit.scene.clear() @@ -149,7 +149,7 @@ class GraspController: self.moveit.gotoL(T_base_grasp * self.T_grasp_ee) rospy.sleep(0.5) self.gripper.grasp() - T_base_retreat = Transform.t([0, 0, 0.05]) * T_base_grasp * self.T_grasp_ee + T_base_retreat = Transform.t_[0, 0, 0.05] * T_base_grasp * self.T_grasp_ee self.moveit.gotoL(T_base_retreat) rospy.sleep(1.0) # Wait to see whether the object slides out of the hand success = self.gripper.read() > 0.002 @@ -194,7 +194,7 @@ class GraspController: rot = T_base_grasp.rotation if rot.as_matrix()[:, 0][0] < 0: # Ensure that the camera is pointing forward T_base_grasp.rotation = rot * Rotation.from_euler("z", np.pi) - T_base_grasp *= Transform.t([0.0, 0.0, 0.01]) + T_base_grasp *= Transform.t_[0.0, 0.0, 0.01] return T_base_grasp def collect_info(self, result): diff --git a/src/active_grasp/policy.py b/src/active_grasp/policy.py index 746af8e..b477405 100644 --- a/src/active_grasp/policy.py +++ b/src/active_grasp/policy.py @@ -69,7 +69,7 @@ class Policy: def calibrate_task_frame(self): xyz = np.r_[self.bbox.center[:2] - 0.15, self.bbox.min[2] - 0.05] - self.T_base_task = Transform.t(xyz) + self.T_base_task = Transform.from_translation(xyz) self.T_task_base = self.T_base_task.inv() tf.broadcast(self.T_base_task, self.base_frame, self.task_frame) rospy.sleep(1.0) # Wait for tf tree to be updated diff --git a/src/active_grasp/rviz.py b/src/active_grasp/rviz.py index 2330045..5a7d5ac 100644 --- a/src/active_grasp/rviz.py +++ b/src/active_grasp/rviz.py @@ -90,9 +90,13 @@ class Visualizer(vgn.rviz.Visualizer): self.draw(markers) - def point(self, frame, point): + def point(self, frame, position): marker = create_sphere_marker( - frame, Transform.t(point), np.full(3, 0.01), [0, 0, 1], "point" + frame, + Transform.from_translation(position), + np.full(3, 0.01), + [0, 0, 1], + "point", ) self.draw([marker]) diff --git a/src/active_grasp/simulation.py b/src/active_grasp/simulation.py index 4c008a0..634fa82 100644 --- a/src/active_grasp/simulation.py +++ b/src/active_grasp/simulation.py @@ -90,9 +90,9 @@ class Simulation: return AABBox(aabb_min, aabb_max) def check_for_grasps(self, bbox): - origin = Transform.t(self.scene.origin) + origin = Transform.from_translation(self.scene.origin) origin.translation[2] -= 0.05 - center = Transform.t(self.scene.center) + center = Transform.from_translation(self.scene.center) # First, reconstruct the scene from many views tsdf = UniformTSDFVolume(self.scene.length, 40)