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

solved the fucking mysterous bug

Browse Source
This commit is contained in:
0nhc 2024-10-21 11:32:13 -05:00
parent c8f0354550
commit d70e585860
5 changed files with 136 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
cfg/active_grasp.rviz
View File

@ -6,7 +6,8 @@ Panels:
Expanded:
- /TF1/Frames1
- /Markers1/Namespaces1
Splitter Ratio: 0.6881287693977356
- /Marker1
Splitter Ratio: 0.6852940917015076
Tree Height: 226
- Class: rviz/Selection
Name: Selection
@ -362,6 +363,42 @@ Visualization Manager:
Transport Hint: raw
Unreliable: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 239; 41; 41
Color Transformer: FlatColor
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Points
Topic: /debug_pcd
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /grasp_markers
Name: Marker
Namespaces:
{}
Queue Size: 100
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
@ -390,7 +427,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 0.7134475111961365
Distance: 0.8019989132881165
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@ -398,17 +435,17 @@ Visualization Manager:
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0.3979946970939636
Y: -0.1718180924654007
Z: 0.29551374912261963
X: 0.5695413947105408
Y: -0.03970015048980713
Z: 0.45675671100616455
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.4653984010219574
Pitch: 0.295397013425827
Target Frame: <Fixed Frame>
Yaw: 1.0903979539871216
Yaw: 5.118584632873535
Saved:
- Class: rviz/Orbit
Distance: 1.2000000476837158
@ -448,5 +485,5 @@ Window Geometry:
Views:
collapsed: true
Width: 1095
X: 1270
Y: 138
X: 1260
Y: 123

2
cfg/active_grasp.yaml
View File

@ -2,7 +2,7 @@ bt_sim:
gui: True
gripper_force: 10
# scene: random
scene: $(find active_grasp)/cfg/sim/challenging_scene_2.yaml
scene: $(find active_grasp)/cfg/sim/challenging_scene_1.yaml
hw:
roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt

2
scripts/run.py
View File

@ -46,7 +46,7 @@ def main():
def create_parser():
parser = argparse.ArgumentParser()
parser.add_argument("policy", type=str, choices=registry.keys())
parser.add_argument("--runs", type=int, default=10)
parser.add_argument("--runs", type=int, default=1)
parser.add_argument("--wait-for-input", action="store_true")
parser.add_argument("--logdir", type=Path, default="logs")
parser.add_argument("--seed", type=int, default=1)

94
src/active_grasp/active_perception_policy.py
View File

@ -12,6 +12,15 @@ import requests
import matplotlib.pyplot as plt
from vgn.grasp import ParallelJawGrasp
import time
from visualization_msgs.msg import Marker, MarkerArray
from geometry_msgs.msg import Pose
import tf
import sensor_msgs.point_cloud2 as pc2
from sensor_msgs.msg import PointCloud2, PointField
import std_msgs.msg
import ros_numpy
class RealTime3DVisualizer:
@ -192,6 +201,10 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
self.updated = False
self._base_url = flask_base_url
# For debugging
self.pcd_publisher = rospy.Publisher('/debug_pcd', PointCloud2, queue_size=10)
self.grasp_publisher = rospy.Publisher("/grasp_markers", MarkerArray, queue_size=10)
def request_grasping_pose(self, data):
response = requests.post(f"{self._base_url}/get_gsnet_grasp", json=data)
@ -247,18 +260,29 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
self.target_points, self.scene_points = self.depth_image_to_ap_input(img, seg, target_id)
target_points_list = np.asarray(self.target_points.cpu().numpy())[0].tolist()
central_point_of_target = np.mean(target_points_list, axis=0)
target_points_radius = np.max(np.linalg.norm(target_points_list - central_point_of_target, axis=1))
scene_points_list = np.asarray(self.scene_points.cpu().numpy())[0].tolist()
merged_points_list = target_points_list + scene_points_list
# gsnet_input_points = self.crop_pts_sphere(np.asarray(merged_points_list), central_point_of_target)
gsnet_input_points = target_points_list
gsnet_input_points = self.crop_pts_sphere(np.asarray(merged_points_list),
central_point_of_target,
radius=target_points_radius)
# gsnet_input_points = target_points_list
# gsnet_input_points = merged_points_list
self.publish_pointcloud(gsnet_input_points)
gsnet_grasping_poses = np.asarray(self.request_grasping_pose(gsnet_input_points))
# DEBUG: publish grasps
# self.publish_grasps(gsnet_grasping_poses)
# Convert all grasping poses' reference frame to arm frame
current_cam_pose = torch.from_numpy(x.as_matrix()).float().to("cuda:0")
for gg in gsnet_grasping_poses:
T = np.asarray(gg['T'])
gg['T'] = current_cam_pose.cpu().numpy() @ T
gg['T'] = current_cam_pose.cpu().numpy().dot(np.asarray(gg['T']))
# Now here is a mysterous bug, the grasping poses have to be rotated
# 90 degrees around y-axis to be in the correct reference frame
R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
gg['T'][:3, :3] = gg['T'][:3, :3].dot(R)
# Convert grasping poses to ParallelJawGrasp objects
grasps = []
qualities = []
@ -271,7 +295,6 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
# Visualize grasps
self.vis.grasps(self.base_frame, grasps, qualities)
time.sleep(1000000)
# Filter grasps
filtered_grasps = []
@ -280,8 +303,8 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
pose = grasp.pose
# tip = pose.rotation.apply([0, 0, 0.05]) + pose.translation
tip = pose.translation
# if self.bbox.is_inside(tip):
if(True):
if self.bbox.is_inside(tip):
# if(True):
q_grasp = self.solve_ee_ik(q, pose * self.T_grasp_ee)
if q_grasp is not None:
filtered_grasps.append(grasp)
@ -294,6 +317,59 @@ class ActivePerceptionSingleViewPolicy(SingleViewPolicy):
self.vis.clear_grasp()
self.done = True
def publish_grasps(self, gg):
marker_array = MarkerArray()
marker_array.markers = []
for idx, g in enumerate(gg):
g['T'] = np.asarray(g['T'])
marker = Marker()
marker.header.frame_id = "camera_depth_optical_frame"
marker.header.stamp = rospy.Time.now()
marker.ns = "grasps"
marker.id = idx
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose.position.x = g['T'][0, 3]
marker.pose.position.y = g['T'][1, 3]
marker.pose.position.z = g['T'][2, 3]
q = tf.transformations.quaternion_from_matrix(g['T'])
marker.pose.orientation.x = q[0]
marker.pose.orientation.y = q[1]
marker.pose.orientation.z = q[2]
marker.pose.orientation.w = q[3]
marker.scale.x = 0.1
marker.scale.y = 0.01
marker.scale.z = 0.01
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker_array.markers.append(marker)
self.grasp_publisher.publish(marker_array)
def publish_pointcloud(self, point_cloud):
point_cloud = np.asarray(point_cloud)
cloud_msg = self.create_pointcloud_msg(point_cloud)
self.pcd_publisher.publish(cloud_msg)
def create_pointcloud_msg(self, point_cloud):
# Define the header
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'camera_depth_optical_frame' # Change this to your desired frame of reference
# Define the fields for the PointCloud2 message
fields = [
PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1),
PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1),
PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1),
]
# Create the PointCloud2 message
cloud_msg = pc2.create_cloud(header, fields, point_cloud)
return cloud_msg
def crop_pts_sphere(self, points, crop_center, radius=0.2):
crop_mask = np.linalg.norm(points - crop_center, axis=1) < radius
return points[crop_mask].tolist()

6
src/active_grasp/controller.py
View File

@ -113,9 +113,6 @@ class GraspController:
while not self.policy.done:
depth_img, seg_image, pose, q = self.get_state()
target_seg_id = self.get_target_id(TargetIDRequest()).id
# sleep 1s
for i in range(self.control_rate*1):
r.sleep()
self.policy.update(depth_img, seg_image, target_seg_id, pose, q)
# Wait for the robot to move to its desired camera pose
moving_to_The_target = True
@ -129,6 +126,9 @@ class GraspController:
if(linear_d < self.move_to_target_threshold):
# Arrived
moving_to_The_target = False
# sleep 3s to wait for the arrival of the robot
secs = 3
for i in range(self.control_rate*secs):
r.sleep()
elif(self.policy.policy_type=="multi_view"):
while not self.policy.done: