hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Approach the grasp from a fixed offset

Browse Source
This commit is contained in:
Michel Breyer 2021-06-08 15:36:02 +02:00
parent 139b73d657
commit 12ae207b5b
1 changed files with 24 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
policies.py
View File

@ -52,6 +52,7 @@ class BaseController:
self.reset() self.reset()
self.explore() self.explore()
self.execute_grasp() self.execute_grasp()
# self.release_object()
def reset(self): def reset(self):
self._reset_env() self._reset_env()
@ -73,26 +74,35 @@ class BaseController:
if not self.best_grasp: if not self.best_grasp:
return return
grasp = self.best_grasp self.gripper.move(0.08)
# Ensure that the camera is pointing forward. # 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: if rot.as_matrix()[:, 0][0] < 0:
grasp.pose.rotation = rot * Rotation.from_euler("z", np.pi) T_O_G.rotation = rot * Rotation.from_euler("z", np.pi)
target = self.T_B_O * grasp.pose * self._ee_grasp_offset.inv()
self.gripper.move(0.08) # Move to an initial pose offset.
self._send_pose_command(target) target = T_O_G * Transform.translation([0, 0, -0.05]) * self.T_G_EE
self._send_target_pose(target)
rospy.sleep(3.0) 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) 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) rospy.sleep(2.0)
def _setup_robot_connection(self): def _setup_robot_connection(self):
self.base_frame = rospy.get_param("~base_frame_id") self.base_frame = rospy.get_param("~base_frame_id")
self.ee_frame = rospy.get_param("~ee_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.target_pose_pub = rospy.Publisher("/command", PoseStamped, queue_size=10)
self.gripper = PandaGripperClient() self.gripper = PandaGripperClient()
@ -180,10 +190,11 @@ class BaseController:
vgn.vis.draw_grasps(grasps, 0.05) vgn.vis.draw_grasps(grasps, 0.05)
return grasps[0] if len(grasps) > 0 else None 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 = PoseStamped()
msg.header.frame_id = self.base_frame 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) self.target_pose_pub.publish(msg)
@ -219,7 +230,7 @@ class FixedTrajectoryBaseline(BaseController):
def _init_policy(self): def _init_policy(self):
self.tic = rospy.Time.now() 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.center = np.r_[x0.translation[0] + self.radius, x0.translation[1:]]
self.target = x0 self.target = x0
@ -238,7 +249,7 @@ class FixedTrajectoryBaseline(BaseController):
self.center self.center
+ np.r_[self.radius * np.cos(t), self.radius * np.sin(t), 0.0] + 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 # Draw
self._draw_scene_cloud() self._draw_scene_cloud()