From 12ae207b5b52db87a5833c3e976a5620c6860716 Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Tue, 8 Jun 2021 15:36:02 +0200 Subject: [PATCH] Approach the grasp from a fixed offset --- policies.py | 37 ++++++++++++++++++++++++------------- 1 file changed, 24 insertions(+), 13 deletions(-) diff --git a/policies.py b/policies.py index 41f0621..d275890 100644 --- a/policies.py +++ b/policies.py @@ -52,6 +52,7 @@ class BaseController: self.reset() self.explore() self.execute_grasp() + # self.release_object() def reset(self): self._reset_env() @@ -73,26 +74,35 @@ class BaseController: if not self.best_grasp: return - grasp = self.best_grasp + self.gripper.move(0.08) # Ensure that the camera is pointing forward. - rot = grasp.pose.rotation + T_O_G = self.best_grasp.pose + rot = T_O_G.rotation if rot.as_matrix()[:, 0][0] < 0: - grasp.pose.rotation = rot * Rotation.from_euler("z", np.pi) - target = self.T_B_O * grasp.pose * self._ee_grasp_offset.inv() + T_O_G.rotation = rot * Rotation.from_euler("z", np.pi) - self.gripper.move(0.08) - self._send_pose_command(target) + # Move to an initial pose offset. + target = T_O_G * Transform.translation([0, 0, -0.05]) * self.T_G_EE + self._send_target_pose(target) rospy.sleep(3.0) + + # Approach grasp pose. + self._send_target_pose(T_O_G * self.T_G_EE) + rospy.sleep(1.0) + + # Close the fingers. self.gripper.move(0.0) - target.translation[2] += 0.3 - self._send_pose_command(target) + + # Lift the object. + target = Transform.translation([0, 0, 0.2]) * T_O_G * self.T_G_EE + self._send_target_pose(target) rospy.sleep(2.0) def _setup_robot_connection(self): self.base_frame = rospy.get_param("~base_frame_id") self.ee_frame = rospy.get_param("~ee_frame_id") - self._ee_grasp_offset = Transform.from_list(rospy.get_param("~ee_grasp_offset")) + self.T_G_EE = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv() self.target_pose_pub = rospy.Publisher("/command", PoseStamped, queue_size=10) self.gripper = PandaGripperClient() @@ -180,10 +190,11 @@ class BaseController: vgn.vis.draw_grasps(grasps, 0.05) return grasps[0] if len(grasps) > 0 else None - def _send_pose_command(self, target): + def _send_target_pose(self, target): + """Target is expected to be given w.r.t. the task frame.""" msg = PoseStamped() msg.header.frame_id = self.base_frame - msg.pose = to_pose_msg(target) + msg.pose = to_pose_msg(self.T_B_O * target) self.target_pose_pub.publish(msg) @@ -219,7 +230,7 @@ class FixedTrajectoryBaseline(BaseController): def _init_policy(self): self.tic = rospy.Time.now() - x0 = tf.lookup(self.base_frame, self.ee_frame) + x0 = tf.lookup(self.frame, self.ee_frame) self.center = np.r_[x0.translation[0] + self.radius, x0.translation[1:]] self.target = x0 @@ -238,7 +249,7 @@ class FixedTrajectoryBaseline(BaseController): self.center + np.r_[self.radius * np.cos(t), self.radius * np.sin(t), 0.0] ) - self._send_pose_command(self.target) + self._send_target_pose(self.target) # Draw self._draw_scene_cloud()