From ecb6da23eb1650f5d36fb0da215cf6b5a1852fcf Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 10:53:23 +0100
Subject: [PATCH 01/13] Launch AprilTag
---
cfg/apriltag.yaml | 12 ++++++++++++
launch/apriltag.launch | 20 ++++++++++++++++++++
launch/hw.launch | 13 +++++++++++++
launch/panda.launch | 6 ------
4 files changed, 45 insertions(+), 6 deletions(-)
create mode 100644 cfg/apriltag.yaml
create mode 100644 launch/apriltag.launch
create mode 100644 launch/hw.launch
diff --git a/cfg/apriltag.yaml b/cfg/apriltag.yaml
new file mode 100644
index 0000000..4a5259f
--- /dev/null
+++ b/cfg/apriltag.yaml
@@ -0,0 +1,12 @@
+ # AprilTag 3 code parameters
+tag_family: 'tag36h11' # options: tagStandard52h13, tagStandard41h12, tag36h11, tag25h9, tag16h5, tagCustom48h12, tagCircle21h7, tagCircle49h12
+tag_threads: 2 # default: 2
+tag_decimate: 1.0 # default: 1.0
+tag_blur: 0.0 # default: 0.0
+tag_refine_edges: 1 # default: 1
+tag_debug: 0 # default: 0
+publish_tf: true # default: false
+
+# Definitions of tags to detect
+standalone_tags: [{id: 0, size: 0.091}]
+tag_bundles: []
diff --git a/launch/apriltag.launch b/launch/apriltag.launch
new file mode 100644
index 0000000..3953d7d
--- /dev/null
+++ b/launch/apriltag.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/hw.launch b/launch/hw.launch
new file mode 100644
index 0000000..5660729
--- /dev/null
+++ b/launch/hw.launch
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/launch/panda.launch b/launch/panda.launch
index b8765ac..7f0aac9 100644
--- a/launch/panda.launch
+++ b/launch/panda.launch
@@ -30,10 +30,4 @@
-
-
-
-
-
-
From e765d5a97435e084223482563963686ff99a8884 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 11:11:57 +0100
Subject: [PATCH 02/13] Read base tag transform
---
scripts/calibrate_roi.py | 15 +++++++++++++++
1 file changed, 15 insertions(+)
create mode 100644 scripts/calibrate_roi.py
diff --git a/scripts/calibrate_roi.py b/scripts/calibrate_roi.py
new file mode 100644
index 0000000..a9d041f
--- /dev/null
+++ b/scripts/calibrate_roi.py
@@ -0,0 +1,15 @@
+import numpy as np
+import rospy
+
+from robot_helpers.ros import tf
+
+
+def main():
+ rospy.init_node("calibrate_roi")
+ tf.init()
+ T_base_roi = tf.lookup("panda_link0", "tag_0")
+ np.savetxt("cfg/T_base_tag.txt", T_base_roi.as_matrix())
+
+
+if __name__ == "__main__":
+ main()
From e7aa81fed3086a44f01bbf536ad8e082403f18ae Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 11:49:50 +0100
Subject: [PATCH 03/13] Minor
---
cfg/hw/T_base_tag.txt | 4 ++++
cfg/{ => hw}/apriltag.yaml | 0
cfg/{ => sim}/mustard.yaml | 0
launch/apriltag.launch | 2 +-
scripts/calibrate_roi.py | 2 +-
src/active_grasp/simulation.py | 2 +-
6 files changed, 7 insertions(+), 3 deletions(-)
create mode 100644 cfg/hw/T_base_tag.txt
rename cfg/{ => hw}/apriltag.yaml (100%)
rename cfg/{ => sim}/mustard.yaml (100%)
diff --git a/cfg/hw/T_base_tag.txt b/cfg/hw/T_base_tag.txt
new file mode 100644
index 0000000..e5055df
--- /dev/null
+++ b/cfg/hw/T_base_tag.txt
@@ -0,0 +1,4 @@
+9.998979853190785860e-01 1.338047579286792167e-02 4.998181909874715480e-03 3.216251487106734364e-01
+-1.341414276126873424e-02 9.998871470166096342e-01 6.764170678302552919e-03 -1.756941528330971392e-01
+-4.907110028114804005e-03 -6.830526959274958038e-03 9.999646314608484632e-01 -3.868988796629513338e-02
+0.000000000000000000e+00 0.000000000000000000e+00 0.000000000000000000e+00 1.000000000000000000e+00
diff --git a/cfg/apriltag.yaml b/cfg/hw/apriltag.yaml
similarity index 100%
rename from cfg/apriltag.yaml
rename to cfg/hw/apriltag.yaml
diff --git a/cfg/mustard.yaml b/cfg/sim/mustard.yaml
similarity index 100%
rename from cfg/mustard.yaml
rename to cfg/sim/mustard.yaml
diff --git a/launch/apriltag.launch b/launch/apriltag.launch
index 3953d7d..551f8eb 100644
--- a/launch/apriltag.launch
+++ b/launch/apriltag.launch
@@ -11,7 +11,7 @@
-
+
diff --git a/scripts/calibrate_roi.py b/scripts/calibrate_roi.py
index a9d041f..ef9991a 100644
--- a/scripts/calibrate_roi.py
+++ b/scripts/calibrate_roi.py
@@ -8,7 +8,7 @@ def main():
rospy.init_node("calibrate_roi")
tf.init()
T_base_roi = tf.lookup("panda_link0", "tag_0")
- np.savetxt("cfg/T_base_tag.txt", T_base_roi.as_matrix())
+ np.savetxt("cfg/hw/T_base_tag.txt", T_base_roi.as_matrix())
if __name__ == "__main__":
diff --git a/src/active_grasp/simulation.py b/src/active_grasp/simulation.py
index 44b5d00..fad78dc 100644
--- a/src/active_grasp/simulation.py
+++ b/src/active_grasp/simulation.py
@@ -158,7 +158,7 @@ class Scene:
class YamlScene(Scene):
def __init__(self, config_name):
super().__init__()
- self.config_path = pkg_root / "cfg" / config_name
+ self.config_path = pkg_root / "cfg/sim" / config_name
def load_config(self):
self.scene = load_cfg(self.config_path)
From d8ea9683db6572576a8b070ea4f8b4cc0e80d7dc Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 13:52:07 +0100
Subject: [PATCH 04/13] Load scene config
---
cfg/active_grasp.yaml | 4 ++++
cfg/hw/scene01.yaml | 4 ++++
launch/env.launch | 6 ++---
scripts/hw_node.py | 40 ++++++++++++++++++++++++----------
src/active_grasp/controller.py | 2 ++
5 files changed, 41 insertions(+), 15 deletions(-)
create mode 100644 cfg/hw/scene01.yaml
diff --git a/cfg/active_grasp.yaml b/cfg/active_grasp.yaml
index 18f82cc..aeb99ca 100644
--- a/cfg/active_grasp.yaml
+++ b/cfg/active_grasp.yaml
@@ -4,6 +4,10 @@ bt_sim:
gripper_force: 10
scene: random
+hw:
+ roi_calib_file: $(find active_grasp)/cfg/hw/T_base_tag.txt
+ scene_file: $(find active_grasp)/cfg/hw/scene01.yaml
+
grasp_controller:
base_frame_id: panda_link0
ee_grasp_offset: [0.0, 0.0, -0.383, 0.924, 0.0, 0.0, 0.065] # offset to panda_link8
diff --git a/cfg/hw/scene01.yaml b/cfg/hw/scene01.yaml
new file mode 100644
index 0000000..a0d12b5
--- /dev/null
+++ b/cfg/hw/scene01.yaml
@@ -0,0 +1,4 @@
+target:
+ min: [0.1, 0.1, 0.0]
+ max: [0.2, 0.2, 0.1]
+q0: [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]
diff --git a/launch/env.launch b/launch/env.launch
index c40f708..6447b62 100644
--- a/launch/env.launch
+++ b/launch/env.launch
@@ -1,6 +1,6 @@
-
+
@@ -11,14 +11,14 @@
-
+
-
+
diff --git a/scripts/hw_node.py b/scripts/hw_node.py
index bdc118f..0daba1d 100755
--- a/scripts/hw_node.py
+++ b/scripts/hw_node.py
@@ -1,24 +1,34 @@
#!/usr/bin/env python3
from controller_manager_msgs.srv import *
+import numpy as np
import rospy
from active_grasp.bbox import AABBox, to_bbox_msg
from active_grasp.srv import *
-
-
+from robot_helpers.io import load_yaml
from robot_helpers.ros.moveit import MoveItClient
+from robot_helpers.ros.panda import PandaGripperClient
+from robot_helpers.spatial import Transform
class HwNode:
def __init__(self):
+ self.load_parameters()
+ self.init_robot_connection()
self.advertise_services()
- self.switch_controller = rospy.ServiceProxy(
+ rospy.spin()
+
+ def load_parameters(self):
+ cfg = rospy.get_param("hw")
+ self.T_base_roi = Transform.from_matrix(np.loadtxt(cfg["roi_calib_file"]))
+ self.scene_config = load_yaml(cfg["scene_file"])
+
+ def init_robot_connection(self):
+ self.switch_to_joint_trajectory_controller = rospy.ServiceProxy(
"controller_manager/switch_controller", SwitchController
)
-
self.moveit = MoveItClient("panda_arm")
- rospy.spin()
def advertise_services(self):
rospy.Service("seed", Seed, self.seed)
@@ -29,18 +39,24 @@ class HwNode:
return SeedResponse()
def reset(self, req):
+ # Move to the initial configuration
+ self.switch_to_joint_trajectory_controller()
+ self.moveit.goto(self.scene_config["q0"])
+
+ # Construct bounding box
+ bbox_min = self.T_base_roi.apply(self.scene_config["target"]["min"])
+ bbox_max = self.T_base_roi.apply(self.scene_config["target"]["max"])
+ bbox = AABBox(bbox_min, bbox_max)
+
+ return ResetResponse(to_bbox_msg(bbox))
+
+ def switch_to_joint_trajectory_controller(self):
req = SwitchControllerRequest()
req.start_controllers = ["position_joint_trajectory_controller"]
req.stop_controllers = ["cartesian_velocity_controller"]
+ req.strictness = 1
self.switch_controller(req)
- # Move to the initial configuration
- self.moveit.goto([0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79])
-
- # Detect target
- bbox = AABBox([0.4, -0.1, 0.0], [0.5, 0.1, 0.1])
- return ResetResponse(to_bbox_msg(bbox))
-
def main():
rospy.init_node("hw")
diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py
index 7185881..68125a0 100644
--- a/src/active_grasp/controller.py
+++ b/src/active_grasp/controller.py
@@ -59,12 +59,14 @@ class GraspController:
req = SwitchControllerRequest()
req.start_controllers = ["cartesian_velocity_controller"]
req.stop_controllers = ["position_joint_trajectory_controller"]
+ req.strictness = 1
self.switch_controller(req)
def switch_to_joint_trajectory_control(self):
req = SwitchControllerRequest()
req.start_controllers = ["position_joint_trajectory_controller"]
req.stop_controllers = ["cartesian_velocity_controller"]
+ req.strictness = 1
self.switch_controller(req)
def init_camera_stream(self):
From c0ae6e2333c502624620a78c76edbf426b9a3e40 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 14:06:26 +0100
Subject: [PATCH 05/13] Fix depth img encoding
---
launch/env.launch | 2 +-
scripts/bt_sim_node.py | 2 +-
src/active_grasp/controller.py | 2 +-
3 files changed, 3 insertions(+), 3 deletions(-)
diff --git a/launch/env.launch b/launch/env.launch
index 6447b62..1413d43 100644
--- a/launch/env.launch
+++ b/launch/env.launch
@@ -1,6 +1,6 @@
-
+
diff --git a/scripts/bt_sim_node.py b/scripts/bt_sim_node.py
index f6947a8..e87cb66 100755
--- a/scripts/bt_sim_node.py
+++ b/scripts/bt_sim_node.py
@@ -317,7 +317,7 @@ class CameraPlugin(Plugin):
if self.cam_noise:
depth = apply_noise(depth)
- msg = self.cv_bridge.cv2_to_imgmsg(depth)
+ msg = self.cv_bridge.cv2_to_imgmsg((1000 * depth).astype(np.uint16))
msg.header.stamp = stamp
self.depth_pub.publish(msg)
diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py
index 68125a0..4a07829 100644
--- a/src/active_grasp/controller.py
+++ b/src/active_grasp/controller.py
@@ -112,7 +112,7 @@ class GraspController:
def get_state(self):
q, _ = self.arm.get_state()
msg = copy.deepcopy(self.latest_depth_msg)
- img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32)
+ img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) * 0.001
pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp)
return img, pose, q
From e6e116bf54c7c29d91b8b7222344aebb26b01a82 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 14:10:32 +0100
Subject: [PATCH 06/13] Open the gripper before each trial
---
scripts/hw_node.py | 3 +++
1 file changed, 3 insertions(+)
diff --git a/scripts/hw_node.py b/scripts/hw_node.py
index 0daba1d..439f09d 100755
--- a/scripts/hw_node.py
+++ b/scripts/hw_node.py
@@ -25,6 +25,7 @@ class HwNode:
self.scene_config = load_yaml(cfg["scene_file"])
def init_robot_connection(self):
+ self.gripper = PandaGripperClient()
self.switch_to_joint_trajectory_controller = rospy.ServiceProxy(
"controller_manager/switch_controller", SwitchController
)
@@ -39,6 +40,8 @@ class HwNode:
return SeedResponse()
def reset(self, req):
+ self.gripper.move(0.04)
+
# Move to the initial configuration
self.switch_to_joint_trajectory_controller()
self.moveit.goto(self.scene_config["q0"])
From 1c3764f785c01a11100e647b7ad77fac27e5d37e Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 14:13:59 +0100
Subject: [PATCH 07/13] Change default value of CL argument
---
scripts/calibrate_roi.py | 2 ++
scripts/run.py | 4 +++-
2 files changed, 5 insertions(+), 1 deletion(-)
mode change 100644 => 100755 scripts/calibrate_roi.py
mode change 100644 => 100755 scripts/run.py
diff --git a/scripts/calibrate_roi.py b/scripts/calibrate_roi.py
old mode 100644
new mode 100755
index ef9991a..2f404f9
--- a/scripts/calibrate_roi.py
+++ b/scripts/calibrate_roi.py
@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+
import numpy as np
import rospy
diff --git a/scripts/run.py b/scripts/run.py
old mode 100644
new mode 100755
index 6a2dbc6..c854ca8
--- a/scripts/run.py
+++ b/scripts/run.py
@@ -1,3 +1,5 @@
+#!/usr/bin/env python3
+
import argparse
from datetime import datetime
import pandas as pd
@@ -33,7 +35,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=100)
+ parser.add_argument("--runs", type=int, default=1)
parser.add_argument("--logdir", type=Path, default="logs")
parser.add_argument("--seed", type=int, default=1)
return parser
From 571b3826547f04fae5f1ff194d058f80d9c7d0ce Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 14:48:51 +0100
Subject: [PATCH 08/13] Open gripper after each run
---
scripts/hw_node.py | 3 +--
src/active_grasp/controller.py | 1 +
2 files changed, 2 insertions(+), 2 deletions(-)
diff --git a/scripts/hw_node.py b/scripts/hw_node.py
index 439f09d..f8c161b 100755
--- a/scripts/hw_node.py
+++ b/scripts/hw_node.py
@@ -40,11 +40,10 @@ class HwNode:
return SeedResponse()
def reset(self, req):
- self.gripper.move(0.04)
-
# Move to the initial configuration
self.switch_to_joint_trajectory_controller()
self.moveit.goto(self.scene_config["q0"])
+ self.gripper.move(0.04)
# Construct bounding box
bbox_min = self.T_base_roi.apply(self.scene_config["target"]["min"])
diff --git a/src/active_grasp/controller.py b/src/active_grasp/controller.py
index 4a07829..9f8850c 100644
--- a/src/active_grasp/controller.py
+++ b/src/active_grasp/controller.py
@@ -87,6 +87,7 @@ class GraspController:
res = self.execute_grasp(grasp)
else:
res = "aborted"
+ self.gripper.move(0.04)
return self.collect_info(res)
def reset(self):
From 3f53671b3c052a034c42455a2bc9ef329eabae0d Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 15:05:19 +0100
Subject: [PATCH 09/13] Update rviz config
---
cfg/active_grasp.rviz | 202 ++++++++++++++++++++++++++++++++++--------
1 file changed, 166 insertions(+), 36 deletions(-)
diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz
index 53b2b0c..23666d4 100644
--- a/cfg/active_grasp.rviz
+++ b/cfg/active_grasp.rviz
@@ -1,11 +1,12 @@
Panels:
- Class: rviz/Displays
- Help Height: 0
+ Help Height: 70
Name: Displays
Property Tree Widget:
- Expanded: ~
- Splitter Ratio: 0.501006007194519
- Tree Height: 419
+ Expanded:
+ - /TF1/Frames1
+ Splitter Ratio: 0.6881287693977356
+ Tree Height: 286
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -18,7 +19,6 @@ Panels:
- Class: rviz/Views
Expanded:
- /Current View1
- - /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
@@ -56,6 +56,52 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
+ camera_color_frame:
+ Value: false
+ camera_color_optical_frame:
+ Value: false
+ camera_depth_frame:
+ Value: false
+ camera_depth_optical_frame:
+ Value: true
+ camera_link:
+ Value: false
+ panda_EE:
+ Value: false
+ panda_K:
+ Value: false
+ panda_NE:
+ Value: false
+ panda_hand:
+ Value: false
+ panda_leftfinger:
+ Value: false
+ panda_link0:
+ Value: false
+ panda_link1:
+ Value: false
+ panda_link2:
+ Value: false
+ panda_link3:
+ Value: false
+ panda_link4:
+ Value: false
+ panda_link5:
+ Value: false
+ panda_link6:
+ Value: false
+ panda_link7:
+ Value: false
+ panda_link8:
+ Value: false
+ panda_rightfinger:
+ Value: false
+ tag_0:
+ Value: false
+ task:
+ Value: false
+ world:
+ Value: false
Marker Alpha: 1
Marker Scale: 0.30000001192092896
Name: TF
@@ -63,7 +109,27 @@ Visualization Manager:
Show Axes: true
Show Names: false
Tree:
- {}
+ world:
+ panda_link0:
+ panda_link1:
+ panda_link2:
+ panda_link3:
+ panda_link4:
+ panda_link5:
+ panda_link6:
+ panda_link7:
+ camera_depth_optical_frame:
+ {}
+ panda_link8:
+ panda_NE:
+ panda_EE:
+ panda_K:
+ {}
+ panda_hand:
+ panda_leftfinger:
+ {}
+ panda_rightfinger:
+ {}
Update Interval: 0
Value: true
- Alpha: 1
@@ -76,6 +142,70 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
+ camera_depth_optical_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ panda_hand:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_leftfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link4:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link5:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link6:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link7:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_link8:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ panda_rightfinger:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
@@ -84,7 +214,7 @@ Visualization Manager:
Visual Enabled: true
- Acceleration_Scaling_Factor: 0.1
Class: moveit_rviz_plugin/MotionPlanning
- Enabled: true
+ Enabled: false
Move Group Namespace: ""
MoveIt_Allow_Approximate_IK: false
MoveIt_Allow_External_Program: false
@@ -137,7 +267,7 @@ Visualization Manager:
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
- Planning Group: hand
+ Planning Group: ""
Query Goal State: true
Query Start State: false
Show Workspace: false
@@ -163,7 +293,7 @@ Visualization Manager:
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
- Value: true
+ Value: false
Velocity_Scaling_Factor: 0.1
- Class: moveit_rviz_plugin/PlanningScene
Enabled: false
@@ -190,18 +320,6 @@ Visualization Manager:
Show Robot Collision: false
Show Robot Visual: false
Value: false
- - Class: rviz/Image
- Enabled: false
- Image Topic: /camera/color/image_raw
- Max Value: 1
- Median window: 5
- Min Value: 0
- Name: ColorImage
- Normalize Range: true
- Queue Size: 2
- Transport Hint: raw
- Unreliable: false
- Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
@@ -214,7 +332,7 @@ Visualization Manager:
Color: 255; 255; 255
Color Transformer: RGB8
Decay Time: 0
- Enabled: false
+ Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
@@ -229,9 +347,21 @@ Visualization Manager:
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
- Value: false
+ Value: true
- Class: rviz/Image
Enabled: true
+ Image Topic: /camera/color/image_raw
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: ColorImage
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ - Class: rviz/Image
+ Enabled: false
Image Topic: /camera/depth/image_rect_raw
Max Value: 1
Median window: 5
@@ -241,7 +371,7 @@ Visualization Manager:
Queue Size: 2
Transport Hint: raw
Unreliable: false
- Value: true
+ Value: false
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: visualization_marker_array
@@ -366,25 +496,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 1.5308048725128174
+ Distance: 1.2000000476837158
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
- Field of View: 0.7853981852531433
+ Field of View: 0.7799999713897705
Focal Point:
- X: 0.34312182664871216
- Y: -0.10157600790262222
- Z: 0.30506056547164917
+ X: 0.5
+ Y: 0
+ Z: 0.4000000059604645
Focal Shape Fixed Size: false
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.4247966408729553
+ Pitch: 0.25999999046325684
Target Frame:
- Yaw: 1.9687141180038452
+ Yaw: 1.0399999618530273
Saved: ~
Window Geometry:
ColorImage:
@@ -393,14 +523,14 @@ Window Geometry:
collapsed: false
Displays:
collapsed: false
- Height: 837
+ Height: 960
Hide Left Dock: false
Hide Right Dock: true
MotionPlanning:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
- QMainWindow State: 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
+ QMainWindow State: 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
Selection:
collapsed: false
Time:
@@ -408,7 +538,7 @@ Window Geometry:
Tool Properties:
collapsed: false
Views:
- collapsed: false
- Width: 1098
- X: 821
+ collapsed: true
+ Width: 1920
+ X: 0
Y: 27
From da73f8211cde01098a6c97aeb4445fc4b7d63f16 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 15:21:09 +0100
Subject: [PATCH 10/13] Cleanup launch files
---
launch/apriltag.launch | 11 ++---------
launch/hw.launch | 32 ++++++++++++++++++++++++++++++--
launch/panda.launch | 33 ---------------------------------
3 files changed, 32 insertions(+), 44 deletions(-)
delete mode 100644 launch/panda.launch
diff --git a/launch/apriltag.launch b/launch/apriltag.launch
index 551f8eb..93a3d6f 100644
--- a/launch/apriltag.launch
+++ b/launch/apriltag.launch
@@ -1,13 +1,6 @@
-
-
-
-
-
-
-
-
-
+
+
diff --git a/launch/hw.launch b/launch/hw.launch
index 5660729..213a100 100644
--- a/launch/hw.launch
+++ b/launch/hw.launch
@@ -1,13 +1,41 @@
+
+
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ [franka_state_controller/joint_states, franka_gripper/joint_states]
+
+
+
+
+
+
+
+
diff --git a/launch/panda.launch b/launch/panda.launch
deleted file mode 100644
index 7f0aac9..0000000
--- a/launch/panda.launch
+++ /dev/null
@@ -1,33 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- [franka_state_controller/joint_states, franka_gripper/joint_states]
-
-
-
-
-
-
-
-
From ae786fc58a52b8c786392f07902e773b8916b2aa Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 15:22:45 +0100
Subject: [PATCH 11/13] Update rviz
---
cfg/active_grasp.rviz | 34 ++++++++++++++++++----------------
1 file changed, 18 insertions(+), 16 deletions(-)
diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz
index 23666d4..9cb5b4d 100644
--- a/cfg/active_grasp.rviz
+++ b/cfg/active_grasp.rviz
@@ -6,7 +6,7 @@ Panels:
Expanded:
- /TF1/Frames1
Splitter Ratio: 0.6881287693977356
- Tree Height: 286
+ Tree Height: 217
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@@ -25,7 +25,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
- SyncSource: DepthImage
+ SyncSource: PointCloud
Preferences:
PromptSaveOnExit: true
Toolbars:
@@ -56,16 +56,8 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
- camera_color_frame:
- Value: false
- camera_color_optical_frame:
- Value: false
- camera_depth_frame:
- Value: false
camera_depth_optical_frame:
Value: true
- camera_link:
- Value: false
panda_EE:
Value: false
panda_K:
@@ -96,10 +88,6 @@ Visualization Manager:
Value: false
panda_rightfinger:
Value: false
- tag_0:
- Value: false
- task:
- Value: false
world:
Value: false
Marker Alpha: 1
@@ -372,6 +360,18 @@ Visualization Manager:
Transport Hint: raw
Unreliable: false
Value: false
+ - Class: rviz/Image
+ Enabled: false
+ Image Topic: /camera/infra1/image_rect_raw
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: InfraImage
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: false
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: visualization_marker_array
@@ -496,7 +496,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 1.2000000476837158
+ Distance: 1.399999976158142
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -526,11 +526,13 @@ Window Geometry:
Height: 960
Hide Left Dock: false
Hide Right Dock: true
+ InfraImage:
+ collapsed: false
MotionPlanning:
collapsed: false
MotionPlanning - Trajectory Slider:
collapsed: false
- QMainWindow State: 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
+ QMainWindow State: 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
Selection:
collapsed: false
Time:
From 7dcb6fb027f9db3d95f36f5d8c4f09067f1b80d6 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 16:11:55 +0100
Subject: [PATCH 12/13] Save default rviz view
---
cfg/active_grasp.rviz | 30 ++++++++++++++++++++++++++----
1 file changed, 26 insertions(+), 4 deletions(-)
diff --git a/cfg/active_grasp.rviz b/cfg/active_grasp.rviz
index 9cb5b4d..e76879b 100644
--- a/cfg/active_grasp.rviz
+++ b/cfg/active_grasp.rviz
@@ -88,6 +88,8 @@ Visualization Manager:
Value: false
panda_rightfinger:
Value: false
+ task:
+ Value: false
world:
Value: false
Marker Alpha: 1
@@ -377,7 +379,7 @@ Visualization Manager:
Marker Topic: visualization_marker_array
Name: Markers
Namespaces:
- {}
+ bbox: true
Queue Size: 100
Value: true
- Alpha: 1
@@ -496,7 +498,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 1.399999976158142
+ Distance: 1.2000000476837158
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -504,7 +506,7 @@ Visualization Manager:
Value: false
Field of View: 0.7799999713897705
Focal Point:
- X: 0.5
+ X: 0.44999998807907104
Y: 0
Z: 0.4000000059604645
Focal Shape Fixed Size: false
@@ -515,7 +517,27 @@ Visualization Manager:
Pitch: 0.25999999046325684
Target Frame:
Yaw: 1.0399999618530273
- Saved: ~
+ Saved:
+ - Class: rviz/Orbit
+ Distance: 1.2000000476837158
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7799999713897705
+ Focal Point:
+ X: 0.44999998807907104
+ Y: 0
+ Z: 0.4000000059604645
+ Focal Shape Fixed Size: false
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Panda
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.25999999046325684
+ Target Frame:
+ Yaw: 1.0399999618530273
Window Geometry:
ColorImage:
collapsed: false
From 542a01de4d5c3480c17f21dbaf10dc1f2969f339 Mon Sep 17 00:00:00 2001
From: Michel Breyer <10465414+mbreyer@users.noreply.github.com>
Date: Fri, 3 Dec 2021 16:51:13 +0100
Subject: [PATCH 13/13] Visualize hw bbox
---
scripts/hw_node.py | 32 +++++++++++++++++++++++---------
1 file changed, 23 insertions(+), 9 deletions(-)
diff --git a/scripts/hw_node.py b/scripts/hw_node.py
index f8c161b..b9f3d77 100755
--- a/scripts/hw_node.py
+++ b/scripts/hw_node.py
@@ -5,6 +5,7 @@ import numpy as np
import rospy
from active_grasp.bbox import AABBox, to_bbox_msg
+from active_grasp.rviz import Visualizer
from active_grasp.srv import *
from robot_helpers.io import load_yaml
from robot_helpers.ros.moveit import MoveItClient
@@ -16,13 +17,13 @@ class HwNode:
def __init__(self):
self.load_parameters()
self.init_robot_connection()
+ self.init_visualizer()
self.advertise_services()
rospy.spin()
def load_parameters(self):
- cfg = rospy.get_param("hw")
- self.T_base_roi = Transform.from_matrix(np.loadtxt(cfg["roi_calib_file"]))
- self.scene_config = load_yaml(cfg["scene_file"])
+ self.cfg = rospy.get_param("hw")
+ self.T_base_roi = Transform.from_matrix(np.loadtxt(self.cfg["roi_calib_file"]))
def init_robot_connection(self):
self.gripper = PandaGripperClient()
@@ -31,6 +32,10 @@ class HwNode:
)
self.moveit = MoveItClient("panda_arm")
+ def init_visualizer(self):
+ self.vis = Visualizer()
+ rospy.Timer(rospy.Duration(1), self.draw_bbox)
+
def advertise_services(self):
rospy.Service("seed", Seed, self.seed)
rospy.Service("reset", Reset, self.reset)
@@ -40,18 +45,23 @@ class HwNode:
return SeedResponse()
def reset(self, req):
+ q0, bbox = self.load_config()
+
# Move to the initial configuration
self.switch_to_joint_trajectory_controller()
- self.moveit.goto(self.scene_config["q0"])
+ self.moveit.goto(q0)
self.gripper.move(0.04)
- # Construct bounding box
- bbox_min = self.T_base_roi.apply(self.scene_config["target"]["min"])
- bbox_max = self.T_base_roi.apply(self.scene_config["target"]["max"])
- bbox = AABBox(bbox_min, bbox_max)
-
return ResetResponse(to_bbox_msg(bbox))
+ def load_config(self):
+ scene_config = load_yaml(self.cfg["scene_file"])
+ q0 = scene_config["q0"]
+ bbox_min = self.T_base_roi.apply(scene_config["target"]["min"])
+ bbox_max = self.T_base_roi.apply(scene_config["target"]["max"])
+ bbox = AABBox(bbox_min, bbox_max)
+ return q0, bbox
+
def switch_to_joint_trajectory_controller(self):
req = SwitchControllerRequest()
req.start_controllers = ["position_joint_trajectory_controller"]
@@ -59,6 +69,10 @@ class HwNode:
req.strictness = 1
self.switch_controller(req)
+ def draw_bbox(self, event):
+ _, bbox = self.load_config()
+ self.vis.bbox("panda_link0", bbox)
+
def main():
rospy.init_node("hw")