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

Update transform interface

Browse Source
This commit is contained in:
Michel Breyer 2022-01-11 11:19:21 +01:00
parent d372f9b82c
commit 7f111252d0
5 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
scripts/hw_node.py
View File

@ -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))

6
src/active_grasp/controller.py
View File

@ -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):

2
src/active_grasp/policy.py
View File

@ -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

8
src/active_grasp/rviz.py
View File

@ -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])

4
src/active_grasp/simulation.py
View File

@ -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)