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

Seed the simulation

Browse Source
This commit is contained in:
Michel Breyer 2021-07-22 11:05:30 +02:00
parent ed40db562e
commit 1aa676f340
17 changed files with 348 additions and 400 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored
View File

@ -128,8 +128,6 @@ dmypy.json
# Pyre type checker
.pyre/
# VSCode
.vscode/
assets/
logs/

1
CMakeLists.txt
View File

@ -16,6 +16,7 @@ add_message_files(
add_service_files(
FILES
Reset.srv
Seed.srv
)
generate_messages(

37
active_grasp/baselines.py
View File

@ -1,10 +1,9 @@
import numpy as np
import scipy.interpolate
from robot_utils.spatial import Transform
import rospy
from active_grasp.policy import BasePolicy
from robot_utils.ros import tf
from robot_helpers.ros import tf
from vgn.utils import look_at
@ -14,9 +13,8 @@ class SingleView(BasePolicy):
"""
def update(self):
self.integrate_latest_image()
self.draw_scene_cloud()
self.best_grasp = self.predict_best_grasp()
self._integrate_latest_image()
self.best_grasp = self._predict_best_grasp()
self.done = True
@ -37,12 +35,10 @@ class TopView(BasePolicy):
error = current.translation - self.target.translation
if np.linalg.norm(error) < 0.01:
self.best_grasp = self.predict_best_grasp()
self.best_grasp = self._predict_best_grasp()
self.done = True
else:
self.integrate_latest_image()
self.draw_scene_cloud()
self.draw_camera_path()
self._integrate_latest_image()
return self.target
@ -71,12 +67,10 @@ class RandomView(BasePolicy):
error = current.translation - self.target.translation
if np.linalg.norm(error) < 0.01:
self.best_grasp = self.predict_best_grasp()
self.best_grasp = self._predict_best_grasp()
self.done = True
else:
self.integrate_latest_image()
self.draw_scene_cloud()
self.draw_camera_path()
self._integrate_latest_image()
return self.target
@ -101,17 +95,15 @@ class FixedTrajectory(BasePolicy):
def update(self):
elapsed_time = (rospy.Time.now() - self.tic).to_sec()
if elapsed_time > self.duration:
self.best_grasp = self.predict_best_grasp()
self.best_grasp = self._predict_best_grasp()
self.done = True
else:
self.integrate_latest_image()
self._integrate_latest_image()
t = self.m(elapsed_time)
eye = self.circle_center + np.r_[self.r * np.cos(t), self.r * np.sin(t), 0]
center = (self.bbox.min + self.bbox.max) / 2.0
up = np.r_[1.0, 0.0, 0.0]
target = self.T_B_task * (self.T_EE_cam * look_at(eye, center, up)).inv()
self.draw_scene_cloud()
self.draw_camera_path()
return target
@ -122,9 +114,8 @@ class AlignmentView(BasePolicy):
def activate(self, bbox):
super().activate(bbox)
self.integrate_latest_image()
self.draw_scene_cloud()
self.best_grasp = self.predict_best_grasp()
self._integrate_latest_image()
self.best_grasp = self._predict_best_grasp()
if self.best_grasp:
R, t = self.best_grasp.rotation, self.best_grasp.translation
center = t
@ -139,10 +130,8 @@ class AlignmentView(BasePolicy):
error = current.translation - self.target.translation
if np.linalg.norm(error) < 0.01:
self.best_grasp = self.predict_best_grasp()
self.best_grasp = self._predict_best_grasp()
self.done = True
else:
self.integrate_latest_image()
self.draw_scene_cloud()
self.draw_camera_path()
self._integrate_latest_image()
return self.target

26
active_grasp/bbox.py Normal file
View File

@ -0,0 +1,26 @@
import numpy as np
import active_grasp.msg
from robot_helpers.ros.conversions import to_point_msg, from_point_msg
class AABBox:
def __init__(self, bbox_min, bbox_max):
self.min = bbox_min
self.max = bbox_max
def is_inside(self, p):
return np.all(p > self.min) and np.all(p < self.max)
def from_bbox_msg(msg):
aabb_min = from_point_msg(msg.min)
aabb_max = from_point_msg(msg.max)
return AABBox(aabb_min, aabb_max)
def to_bbox_msg(bbox):
msg = active_grasp.msg.AABBox()
msg.min = to_point_msg(bbox.min)
msg.max = to_point_msg(bbox.max)
return msg

92
active_grasp/controller.py
View File

@ -1,64 +1,69 @@
from geometry_msgs.msg import PoseStamped
import numpy as np
import rospy
import time
from active_grasp.bbox import from_bbox_msg
from active_grasp.srv import Reset, ResetRequest
from active_grasp.utils import *
from robot_utils.ros.panda import PandaGripperClient
from robot_utils.spatial import Rotation, Transform
from robot_helpers.ros.conversions import to_pose_stamped_msg
from robot_helpers.ros.panda import PandaGripperClient
from robot_helpers.spatial import Rotation, Transform
class GraspController:
def __init__(self, policy):
self.policy = policy
self.controller = CartesianPoseControllerClient()
self.gripper = PandaGripperClient()
self.reset_env = rospy.ServiceProxy("reset", Reset)
self.load_parameters()
self._reset_env = rospy.ServiceProxy("reset", Reset)
self._load_parameters()
self._init_robot_control()
def load_parameters(self):
def _load_parameters(self):
self.T_G_EE = Transform.from_list(rospy.get_param("~ee_grasp_offset")).inv()
def run(self):
bbox = self.reset()
with Timer("exploration_time"):
grasp = self.explore(bbox)
with Timer("execution_time"):
res = self.execute_grasp(grasp)
return self.collect_info(res)
def _init_robot_control(self):
self.target_pose_pub = rospy.Publisher("command", PoseStamped, queue_size=10)
self.gripper = PandaGripperClient()
def reset(self):
req = ResetRequest()
res = self.reset_env(req)
def _send_cmd(self, pose):
msg = to_pose_stamped_msg(pose, "panda_link0")
self.target_pose_pub.publish(msg)
def run(self):
bbox = self._reset()
with Timer("search_time"):
grasp = self._search_grasp(bbox)
res = self._execute_grasp(grasp)
return self._collect_info(res)
def _reset(self):
res = self._reset_env(ResetRequest())
rospy.sleep(1.0) # wait for states to be updated
return from_bbox_msg(res.bbox)
def explore(self, bbox):
def _search_grasp(self, bbox):
self.policy.activate(bbox)
r = rospy.Rate(self.policy.rate)
while not self.policy.done:
while True:
cmd = self.policy.update()
if self.policy.done:
break
self.controller.send_target(cmd)
self._send_cmd(cmd)
r.sleep()
return self.policy.best_grasp
def execute_grasp(self, grasp):
def _execute_grasp(self, grasp):
if not grasp:
return "aborted"
T_B_G = self.postprocess(grasp)
T_B_G = self._postprocess(grasp)
self.gripper.move(0.08)
# Move to an initial pose offset.
self.controller.send_target(
T_B_G * Transform.translation([0, 0, -0.05]) * self.T_G_EE
)
self._send_cmd(T_B_G * Transform.translation([0, 0, -0.05]) * self.T_G_EE)
rospy.sleep(3.0)
# Approach grasp pose.
self.controller.send_target(T_B_G * self.T_G_EE)
self._send_cmd(T_B_G * self.T_G_EE)
rospy.sleep(2.0)
# Close the fingers.
@ -66,7 +71,7 @@ class GraspController:
# Lift the object.
target = Transform.translation([0, 0, 0.2]) * T_B_G * self.T_G_EE
self.controller.send_target(target)
self._send_cmd(target)
rospy.sleep(2.0)
# Check whether the object remains in the hand
@ -74,22 +79,41 @@ class GraspController:
return "succeeded" if success else "failed"
def postprocess(self, T_B_G):
def _postprocess(self, T_B_G):
# Ensure that the camera is pointing forward.
rot = T_B_G.rotation
if rot.as_matrix()[:, 0][0] < 0:
T_B_G.rotation = rot * Rotation.from_euler("z", np.pi)
return T_B_G
def collect_info(self, result):
def _collect_info(self, result):
points = [p.translation for p in self.policy.viewpoints]
d = np.sum([np.linalg.norm(p2 - p1) for p1, p2 in zip(points, points[1:])])
info = {
"result": result,
"viewpoint_count": len(points),
"distance_travelled": d,
"distance": d,
}
info.update(self.policy.info)
info.update(Timer.timers)
return info
class Timer:
timers = dict()
def __init__(self, name):
self.name = name
def __enter__(self):
self.start()
return self
def __exit__(self, *exc_info):
self.stop()
def start(self):
self.tic = time.perf_counter()
def stop(self):
elapsed_time = time.perf_counter() - self.tic
self.timers[self.name] = elapsed_time

148
active_grasp/policy.py
View File

@ -2,35 +2,37 @@ import cv_bridge
import numpy as np
from pathlib import Path
import rospy
from rospy import Publisher
import sensor_msgs.msg
from visualization_msgs.msg import Marker, MarkerArray
from sensor_msgs.msg import CameraInfo, Image, PointCloud2
from robot_utils.perception import Image
from robot_utils.ros import tf
from robot_utils.ros.conversions import *
from robot_utils.ros.rviz import *
from robot_utils.spatial import Transform
from .visualization import Visualizer
from robot_helpers.ros import tf
from robot_helpers.ros.conversions import *
from vgn.detection import VGN, compute_grasps
from vgn.perception import UniformTSDFVolume
from vgn.utils import *
class BasePolicy:
class Policy:
def activate(self, bbox):
raise NotImplementedError
def update(self):
raise NotImplementedError
class BasePolicy(Policy):
def __init__(self):
self.cv_bridge = cv_bridge.CvBridge()
self.vgn = VGN(Path(rospy.get_param("vgn/model")))
self.finger_depth = 0.05
self.load_parameters()
self.lookup_transforms()
self.connect_to_camera()
self.connect_to_rviz()
self.rate = 5
self.info = {}
self._load_parameters()
self._lookup_transforms()
self._init_camera_stream()
self._init_publishers()
self._init_visualizer()
def load_parameters(self):
def _load_parameters(self):
self.task_frame = rospy.get_param("~frame_id")
self.base_frame = rospy.get_param("~base_frame_id")
self.ee_frame = rospy.get_param("~ee_frame_id")
@ -38,114 +40,64 @@ class BasePolicy:
self.info_topic = rospy.get_param("~camera/info_topic")
self.depth_topic = rospy.get_param("~camera/depth_topic")
def lookup_transforms(self):
tf._init_listener()
rospy.sleep(1.0) # wait to receive transforms
def _lookup_transforms(self):
self.T_B_task = tf.lookup(self.base_frame, self.task_frame)
self.T_EE_cam = tf.lookup(self.ee_frame, self.cam_frame)
def connect_to_camera(self):
msg = rospy.wait_for_message(
self.info_topic, sensor_msgs.msg.CameraInfo, rospy.Duration(2.0)
)
def _init_camera_stream(self):
msg = rospy.wait_for_message(self.info_topic, CameraInfo, rospy.Duration(2.0))
self.intrinsic = from_camera_info_msg(msg)
rospy.Subscriber(
self.depth_topic, sensor_msgs.msg.Image, self.sensor_cb, queue_size=1
)
rospy.Subscriber(self.depth_topic, Image, self._sensor_cb, queue_size=1)
def sensor_cb(self, msg):
self.img = Image(depth=self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32))
self.extrinsic = tf.lookup(
self.cam_frame,
self.task_frame,
msg.header.stamp,
rospy.Duration(0.2),
)
def _sensor_cb(self, msg):
self.img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32)
self.extrinsic = tf.lookup(self.cam_frame, self.task_frame, msg.header.stamp)
def connect_to_rviz(self):
self.bbox_pub = Publisher("bbox", Marker, queue_size=1, latch=True)
self.cloud_pub = Publisher("cloud", PointCloud2, queue_size=1, latch=True)
self.path_pub = Publisher("path", MarkerArray, queue_size=1, latch=True)
self.grasps_pub = Publisher("grasps", MarkerArray, queue_size=1, latch=True)
def _init_publishers(self):
self.scene_cloud_pub = rospy.Publisher("scene_cloud", PointCloud2, queue_size=1)
def _init_visualizer(self):
self.visualizer = Visualizer(self.task_frame)
def activate(self, bbox):
self.clear_grasps()
self.bbox = bbox
self.draw_bbox()
self.tsdf = UniformTSDFVolume(0.3, 40)
self.viewpoints = []
self.done = False
self.best_grasp = None # grasp pose defined w.r.t. the robot's base frame
self.visualizer.clear()
self.visualizer.bbox(bbox)
def update(self):
raise NotImplementedError
def integrate_latest_image(self):
def _integrate_latest_image(self):
self.viewpoints.append(self.extrinsic.inv())
self.tsdf.integrate(
self.img,
self.intrinsic,
self.extrinsic,
)
self._publish_scene_cloud()
self.visualizer.path(self.viewpoints)
def predict_best_grasp(self):
def _publish_scene_cloud(self):
cloud = self.tsdf.get_scene_cloud()
msg = to_cloud_msg(self.task_frame, np.asarray(cloud.points))
self.scene_cloud_pub.publish(msg)
def _predict_best_grasp(self):
tsdf_grid = self.tsdf.get_grid()
out = self.vgn.predict(tsdf_grid)
score_fn = lambda g: g.pose.translation[2]
grasps = compute_grasps(self.tsdf.voxel_size, out, score_fn, max_filter_size=3)
grasps = self.filter_grasps_on_target_object(grasps)
self.draw_grasps(grasps)
grasps = self._select_grasps_on_target_object(grasps)
return self.T_B_task * grasps[0].pose if len(grasps) > 0 else None
def filter_grasps_on_target_object(self, grasps):
return [
g
for g in grasps
if self.bbox.is_inside(
g.pose.rotation.apply([0, 0, 0.05]) + g.pose.translation
)
]
def clear_grasps(self):
self.grasps_pub.publish(DELETE_MARKER_ARRAY_MSG)
def draw_bbox(self):
pose = Transform.translation((self.bbox.min + self.bbox.max) / 2.0)
scale = self.bbox.max - self.bbox.min
color = np.r_[0.8, 0.2, 0.2, 0.6]
msg = create_marker(Marker.CUBE, self.task_frame, pose, scale, color)
self.bbox_pub.publish(msg)
def draw_scene_cloud(self):
cloud = self.tsdf.get_scene_cloud()
msg = to_cloud_msg(self.task_frame, np.asarray(cloud.points))
self.cloud_pub.publish(msg)
def draw_grasps(self, grasps):
msg = create_grasp_marker_array(self.task_frame, grasps, self.finger_depth)
self.grasps_pub.publish(msg)
def draw_camera_path(self):
identity = Transform.identity()
color = np.r_[31, 119, 180] / 255.0
# Spheres for each viewpoint
scale = 0.01 * np.ones(3)
spheres = create_marker(
Marker.SPHERE_LIST, self.task_frame, identity, scale, color
)
spheres.id = 0
spheres.points = [to_point_msg(p.translation) for p in self.viewpoints]
# Line strip connecting viewpoints
scale = [0.005, 0.0, 0.0]
lines = create_marker(
Marker.LINE_STRIP, self.task_frame, identity, scale, color
)
lines.id = 1
lines.points = [to_point_msg(p.translation) for p in self.viewpoints]
self.path_pub.publish(MarkerArray([spheres, lines]))
def _select_grasps_on_target_object(self, grasps):
result = []
for g in grasps:
tip = g.pose.rotation.apply([0, 0, 0.05]) + g.pose.translation
if self.bbox.is_inside(tip):
result.append(g)
return result
registry = {}

47
active_grasp/simulation.py
View File

@ -1,31 +1,40 @@
from pathlib import Path
import pybullet as p
import pybullet_data
import rospkg
import time
from active_grasp.utils import *
from robot_utils.bullet import *
from robot_utils.spatial import Rotation, Transform
from active_grasp.bbox import AABBox
from robot_helpers.bullet import *
from robot_helpers.model import Model
from robot_helpers.spatial import Rotation, Transform
class Simulation(BtSim):
def __init__(self, gui, rng):
super().__init__(gui=gui, sleep=False)
self.rng = rng
self.object_uids = []
class Simulation:
def __init__(self, gui):
self.configure_physics_engine(gui, 60, 4)
self.configure_visualizer()
self.find_object_urdfs()
self.find_urdfs()
self.load_table()
self.load_robot()
self.load_controller()
self.object_uids = []
def configure_physics_engine(self, gui, rate, sub_step_count):
self.rate = rate
self.dt = 1.0 / self.rate
p.connect(p.GUI if gui else p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setPhysicsEngineParameter(fixedTimeStep=self.dt, numSubSteps=sub_step_count)
p.setGravity(0.0, 0.0, -9.81)
def configure_visualizer(self):
# p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
p.resetDebugVisualizerCamera(1.4, 50, -35, [0.0, 0.0, 0.6])
def find_object_urdfs(self):
def find_urdfs(self):
rospack = rospkg.RosPack()
assets_path = Path(rospack.get_path("active_grasp")) / "assets"
self.panda_urdf = assets_path / "urdfs/franka/panda_arm_hand.urdf"
root = Path(rospack.get_path("vgn")) / "assets/urdfs/packed/test"
self.urdfs = [str(f) for f in root.iterdir() if f.suffix == ".urdf"]
@ -38,9 +47,9 @@ class Simulation(BtSim):
def load_robot(self):
self.T_W_B = Transform(Rotation.identity(), np.r_[-0.6, 0.0, 0.4])
self.arm = BtPandaArm(self.T_W_B)
self.arm = BtPandaArm(self.panda_urdf, self.T_W_B)
self.gripper = BtPandaGripper(self.arm)
self.model = Model(self.arm.urdf_path, self.arm.base_frame, self.arm.ee_frame)
self.model = Model(self.panda_urdf, self.arm.base_frame, self.arm.ee_frame)
self.camera = BtCamera(320, 240, 1.047, 0.1, 1.0, self.arm.uid, 11)
def load_controller(self):
@ -48,6 +57,9 @@ class Simulation(BtSim):
x0 = self.model.pose(self.arm.ee_frame, q)
self.controller = CartesianPoseController(self.model, self.arm.ee_frame, x0)
def seed(self, seed):
self.rng = np.random.default_rng(seed)
def reset(self):
self.remove_all_objects()
self.set_initial_arm_configuration()
@ -55,6 +67,9 @@ class Simulation(BtSim):
uid = self.select_target()
return self.get_target_bbox(uid)
def step(self):
p.stepSimulation()
def set_initial_arm_configuration(self):
q = [
self.rng.uniform(-0.17, 0.17), # 0.0
@ -110,8 +125,8 @@ class Simulation(BtSim):
self.remove_object(uid)
def select_target(self):
img = self.camera.get_image()
uids, counts = np.unique(img.mask, return_counts=True)
_, _, mask = self.camera.get_image()
uids, counts = np.unique(mask, return_counts=True)
mask = np.isin(uids, self.object_uids) # remove ids of the floor, table, etc
uids, counts = uids[mask], counts[mask]
target_uid = uids[np.argmin(counts)]

71
active_grasp/utils.py
View File

@ -1,71 +0,0 @@
from datetime import datetime
from geometry_msgs.msg import PoseStamped
import pandas as pd
import rospy
import time
import active_grasp.msg
from robot_utils.ros.conversions import *
class CartesianPoseControllerClient:
def __init__(self, topic="/command"):
self.target_pub = rospy.Publisher(topic, PoseStamped, queue_size=10)
def send_target(self, pose):
msg = to_pose_stamped_msg(pose, "panda_link0")
self.target_pub.publish(msg)
class AABBox:
def __init__(self, bbox_min, bbox_max):
self.min = bbox_min
self.max = bbox_max
def is_inside(self, p):
return np.all(p > self.min) and np.all(p < self.max)
def from_bbox_msg(msg):
aabb_min = from_point_msg(msg.min)
aabb_max = from_point_msg(msg.max)
return AABBox(aabb_min, aabb_max)
def to_bbox_msg(bbox):
msg = active_grasp.msg.AABBox()
msg.min = to_point_msg(bbox.min)
msg.max = to_point_msg(bbox.max)
return msg
class Timer:
timers = dict()
def __init__(self, name):
self.name = name
def __enter__(self):
self.start()
return self
def __exit__(self, *exc_info):
self.stop()
def start(self):
self.tic = time.perf_counter()
def stop(self):
elapsed_time = time.perf_counter() - self.tic
self.timers[self.name] = elapsed_time
class Logger:
def __init__(self, logdir, policy, desc):
stamp = datetime.now().strftime("%y%m%d-%H%M%S")
name = "{}_policy={},{}".format(stamp, policy, desc).strip(",")
self.path = logdir / (name + ".csv")
def log_run(self, info):
df = pd.DataFrame.from_records([info])
df.to_csv(self.path, mode="a", header=not self.path.exists(), index=False)

49
active_grasp/visualization.py Normal file
View File

@ -0,0 +1,49 @@
import numpy as np
import rospy
from robot_helpers.ros.rviz import *
from robot_helpers.spatial import Transform
class Visualizer:
def __init__(self, frame, topic="visualization_marker_array"):
self.frame = frame
self.marker_pub = rospy.Publisher(topic, MarkerArray, queue_size=1)
def clear(self):
marker = Marker(action=Marker.DELETEALL)
self.draw([marker])
def bbox(self, bbox):
pose = Transform.translation((bbox.min + bbox.max) / 2.0)
scale = bbox.max - bbox.min
color = np.r_[0.8, 0.2, 0.2, 0.6]
marker = create_cube_marker(self.frame, pose, scale, color, ns="bbox")
self.draw([marker])
def path(self, poses):
color = np.r_[31, 119, 180] / 255.0
points = [p.translation for p in poses]
spheres = create_sphere_list_marker(
self.frame,
Transform.identity(),
np.full(3, 0.01),
color,
points,
"path",
0,
)
lines = create_line_strip_marker(
self.frame,
Transform.identity(),
[0.005, 0.0, 0.0],
color,
points,
"path",
1,
)
self.draw([spheres, lines])
def draw(self, markers):
self.marker_pub.publish(MarkerArray(markers=markers))

118
launch/active_grasp.rviz → config/active_grasp.rviz
View File

@ -6,7 +6,7 @@ Panels:
Expanded:
- /TF1/Tree1
Splitter Ratio: 0.5
Tree Height: 595
Tree Height: 574
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
@ -25,7 +25,7 @@ Panels:
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
SyncSource: SceneCloud
Preferences:
PromptSaveOnExit: true
Toolbars:
@ -61,7 +61,7 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
cam_optical_frame:
camera_optical_frame:
Alpha: 1
Show Axes: false
Show Trail: false
@ -136,8 +136,8 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: false
cam_optical_frame:
Value: false
camera_optical_frame:
Value: true
panda_hand:
Value: false
panda_leftfinger:
@ -173,57 +173,27 @@ Visualization Manager:
Show Axes: true
Show Names: true
Tree:
panda_link0:
panda_link1:
panda_link2:
panda_link3:
panda_link4:
panda_link5:
panda_link6:
panda_link7:
panda_link8:
panda_hand:
camera_optical_frame:
{}
panda_leftfinger:
{}
panda_rightfinger:
{}
world:
panda_link0:
panda_link1:
panda_link2:
panda_link3:
panda_link4:
panda_link5:
panda_link6:
panda_link7:
panda_link8:
panda_hand:
cam_optical_frame:
{}
panda_leftfinger:
{}
panda_rightfinger:
{}
task:
{}
Update Interval: 0
Value: true
- Alpha: 0.20000000298023224
Autocompute Intensity Bounds: false
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 4096
Min Color: 0; 0; 0
Min Intensity: 0
Name: TSDF
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.007499999832361937
Style: Boxes
Topic: /tsdf
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
@ -247,7 +217,7 @@ Visualization Manager:
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Spheres
Topic: /cloud
Topic: /scene_cloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
@ -268,28 +238,12 @@ Visualization Manager:
Topic: /command
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /bbox
Name: BBox
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /path
Name: CameraPath
Marker Topic: visualization_marker_array
Name: Markers
Namespaces:
{}
Queue Size: 100
Value: true
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /grasps
Name: Grasps
Namespaces:
{}
bbox: true
Queue Size: 100
Value: true
Enabled: true
@ -320,7 +274,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 1.5774216651916504
Distance: 1.3517695665359497
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@ -328,25 +282,25 @@ Visualization Manager:
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0.2026568502187729
Y: 0.13089965283870697
Z: 0.3290979564189911
X: 0.3073185980319977
Y: 0.050485748797655106
Z: 0.3944588601589203
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.3197976350784302
Pitch: 0.4747979938983917
Target Frame: <Fixed Frame>
Yaw: 5.398510456085205
Yaw: 5.098489761352539
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 892
Height: 871
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd000000040000000000000156000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004fb0000003efc0100000002fb0000000800540069006d00650100000000000004fb000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000039f000002de00000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd000000040000000000000156000002c9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002c9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000050e0000003efc0100000002fb0000000800540069006d006501000000000000050e000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003b2000002c900000004000000040000000800000008fc00000002000000020000000000000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@ -355,6 +309,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: true
Width: 1275
X: 61
Y: 163
Width: 1294
X: 104
Y: 374

1
launch/active_grasp.yaml → config/active_grasp.yaml
View File

@ -1,6 +1,5 @@
bt_sim:
gui: True
seed: 12
active_grasp:
frame_id: task

7
launch/active_grasp.launch
View File

@ -2,11 +2,12 @@
<launch>
<arg name="launch_rviz" default="false" />
<rosparam command="load" file="$(find active_grasp)launch/active_grasp.yaml" subst_value="true" />
<rosparam command="load" file="$(find active_grasp)/config/active_grasp.yaml" subst_value="true" />
<param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/urdfs/franka/panda_arm_hand.urdf.xacro" />
<param name="robot_description" command="$(find xacro)/xacro $(find active_grasp)/assets/urdfs/panda_arm_hand.urdf.xacro" />
<node pkg="active_grasp" type="bt_sim_node.py" name="bt_sim" output="screen" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/active_grasp.rviz" if="$(arg launch_rviz)" />
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/config/active_grasp.rviz" if="$(arg launch_rviz)" />
</launch>

25
notebooks/print_metrics.py
View File

@ -1,25 +0,0 @@
#%%
import pandas as pd
#%%
logfile = "../logs/210712-132211_policy=top.csv"
df = pd.read_csv(logfile)
#%%
metrics = [
("Runs", len(df.index)),
("", ""),
("Succeeded", (df.result == "succeeded").sum()),
("Failed", (df.result == "failed").sum()),
("Aborted", (df.result == "aborted").sum()),
("", ""),
("Success rate", round((df.result == "succeeded").mean(), 2)),
("Mean time", round(df.exploration_time.mean(), 2)),
("Mean distance", round(df.distance_travelled.mean(), 2)),
("Mean viewpoints", round(df.viewpoint_count.mean())),
]
for k, v in metrics:
print("{:<16} {:>8}".format(k, v))
# %%

4
requirements.txt
View File

@ -0,0 +1,4 @@
numpy
pandas
pybullet
tqdm

93
scripts/bt_sim_node.py
View File

@ -10,20 +10,16 @@ from sensor_msgs.msg import JointState, Image, CameraInfo
from threading import Thread
import tf2_ros
from active_grasp.srv import Reset, ResetResponse
from active_grasp.bbox import to_bbox_msg
from active_grasp.srv import *
from active_grasp.simulation import Simulation
from active_grasp.utils import *
from robot_utils.ros.conversions import *
from robot_helpers.ros.conversions import *
class BtSimNode:
def __init__(self):
self.gui = rospy.get_param("~gui", True)
seed = rospy.get_param("~seed", None)
rng = np.random.default_rng(seed) if seed else np.random
self.sim = Simulation(gui=self.gui, rng=rng)
self.sim = Simulation(gui=self.gui)
self._init_plugins()
self._advertise_services()
self._broadcast_transforms()
@ -33,11 +29,13 @@ class BtSimNode:
PhysicsPlugin(self.sim),
JointStatePlugin(self.sim.arm, self.sim.gripper),
ArmControllerPlugin(self.sim.arm, self.sim.controller),
GripperControllerPlugin(self.sim.gripper),
MoveActionPlugin(self.sim.gripper),
GraspActionPlugin(self.sim.gripper),
CameraPlugin(self.sim.camera),
]
def _advertise_services(self):
rospy.Service("seed", Seed, self.seed)
rospy.Service("reset", Reset, self.reset)
def _broadcast_transforms(self):
@ -50,14 +48,16 @@ class BtSimNode:
]
self.static_broadcaster.sendTransform(msgs)
def seed(self, req):
self.sim.seed(req.seed)
return SeedResponse()
def reset(self, req):
for plugin in self.plugins:
plugin.is_running = False
rospy.sleep(1.0) # TODO replace with a read-write lock
bbox = self.sim.reset()
res = ResetResponse(to_bbox_msg(bbox))
for plugin in self.plugins:
plugin.is_running = True
return res
@ -137,45 +137,54 @@ class ArmControllerPlugin(Plugin):
self.arm.set_desired_joint_velocities(cmd)
class GripperControllerPlugin(Plugin):
class MoveActionPlugin(Plugin):
def __init__(self, gripper, rate=10):
super().__init__(rate)
self.gripper = gripper
self.dt = 1.0 / self.rate
self._init_move_action_server()
self._init_grasp_action_server()
self._init_action_server()
def _init_move_action_server(self):
def _init_action_server(self):
name = "/franka_gripper/move"
self.move_server = SimpleActionServer(name, MoveAction, auto_start=False)
self.move_server.register_goal_callback(self._move_action_goal_cb)
self.move_server.start()
self.action_server = SimpleActionServer(name, MoveAction, auto_start=False)
self.action_server.register_goal_callback(self._action_goal_cb)
self.action_server.start()
def _init_grasp_action_server(self):
name = "/franka_gripper/grasp"
self.grasp_server = SimpleActionServer(name, GraspAction, auto_start=False)
self.grasp_server.register_goal_callback(self._grasp_action_goal_cb)
self.grasp_server.start()
def _move_action_goal_cb(self):
self.elapsed_time_since_move_action_goal = 0.0
goal = self.move_server.accept_new_goal()
self.gripper.set_desired_width(goal.width)
def _grasp_action_goal_cb(self):
self.elapsed_time_since_grasp_action_goal = 0.0
goal = self.grasp_server.accept_new_goal()
def _action_goal_cb(self):
self.elapsed_time = 0.0
goal = self.action_server.accept_new_goal()
self.gripper.set_desired_width(goal.width)
def _update(self):
if self.move_server.is_active():
self.elapsed_time_since_move_action_goal += self.dt
if self.elapsed_time_since_move_action_goal > 1.0:
self.move_server.set_succeeded()
if self.grasp_server.is_active():
self.elapsed_time_since_grasp_action_goal += self.dt
if self.elapsed_time_since_grasp_action_goal > 1.0:
self.grasp_server.set_succeeded()
if self.action_server.is_active():
self.elapsed_time += self.dt
if self.elapsed_time > 1.0:
self.action_server.set_succeeded()
class GraspActionPlugin(Plugin):
def __init__(self, gripper, rate=10):
super().__init__(rate)
self.gripper = gripper
self.dt = 1.0 / self.rate
self._init_action_server()
def _init_action_server(self):
name = "/franka_gripper/grasp"
self.action_server = SimpleActionServer(name, GraspAction, auto_start=False)
self.action_server.register_goal_callback(self._action_goal_cb)
self.action_server.start()
def _action_goal_cb(self):
self.elapsed_time = 0.0
goal = self.action_server.accept_new_goal()
self.gripper.set_desired_width(goal.width)
def _update(self):
if self.action_server.is_active():
self.elapsed_time += self.dt
if self.elapsed_time > 1.0:
self.action_server.set_succeeded()
class CameraPlugin(Plugin):
@ -201,8 +210,8 @@ class CameraPlugin(Plugin):
msg.header.stamp = stamp
self.info_pub.publish(msg)
img = self.camera.get_image()
msg = self.cv_bridge.cv2_to_imgmsg(img.depth)
_, depth, _ = self.camera.get_image()
msg = self.cv_bridge.cv2_to_imgmsg(depth)
msg.header.stamp = stamp
self.depth_pub.publish(msg)

25
scripts/run.py
View File

@ -1,10 +1,24 @@
import argparse
from datetime import datetime
import pandas as pd
from pathlib import Path
import rospy
from tqdm import tqdm
from active_grasp.controller import *
from active_grasp.policy import make, registry
from active_grasp.srv import Seed
class Logger:
def __init__(self, logdir, policy):
stamp = datetime.now().strftime("%y%m%d-%H%M%S")
name = "{}_policy={}".format(stamp, policy)
self.path = logdir / (name + ".csv")
def log_run(self, info):
df = pd.DataFrame.from_records([info])
df.to_csv(self.path, mode="a", header=not self.path.exists(), index=False)
def create_parser():
@ -12,17 +26,24 @@ def create_parser():
parser.add_argument("policy", type=str, choices=registry.keys())
parser.add_argument("--runs", type=int, default=10)
parser.add_argument("--logdir", type=Path, default="logs")
parser.add_argument("--desc", type=str, default="")
parser.add_argument("--seed", type=int, default=12)
return parser
def seed_simulation(seed):
rospy.ServiceProxy("seed", Seed)(seed)
rospy.sleep(1.0)
def main():
rospy.init_node("active_grasp")
parser = create_parser()
args = parser.parse_args()
policy = make(args.policy)
controller = GraspController(policy)
logger = Logger(args.logdir, args.policy, args.desc)
logger = Logger(args.logdir, args.policy)
seed_simulation(args.seed)
for _ in tqdm(range(args.runs)):
info = controller.run()

2
srv/Seed.srv Normal file
View File

@ -0,0 +1,2 @@
uint32 seed
---