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

Move initial config sampling to the scene

Browse Source
This commit is contained in:
Michel Breyer 2021-09-07 23:18:10 +02:00
parent fa4b0f07ad
commit 60443b0c2f
2 changed files with 13 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
active_grasp/controller.py
View File

@ -50,9 +50,7 @@ class GraspController:
def init_moveit(self): def init_moveit(self):
self.moveit = MoveItClient("panda_arm") self.moveit = MoveItClient("panda_arm")
rospy.sleep(1.0) # wait for connections to be established rospy.sleep(1.0) # wait for connections to be established
msg = to_pose_stamped_msg(Transform.t([0.4, 0, 0.4]), self.base_frame)
table_height = 0.22
msg = to_pose_stamped_msg(Transform.t([0.4, 0, table_height]), self.base_frame)
self.moveit.scene.add_box("table", msg, size=(0.5, 0.5, 0.02)) self.moveit.scene.add_box("table", msg, size=(0.5, 0.5, 0.02))
def switch_to_cartesian_velocity_control(self): def switch_to_cartesian_velocity_control(self):

26
active_grasp/simulation.py
View File

@ -46,21 +46,13 @@ class Simulation:
def reset(self): def reset(self):
self.scene.reset(rng=self.rng) self.scene.reset(rng=self.rng)
self.set_initial_arm_configuration() q = self.scene.sample_initial_configuration(self.rng)
self.set_arm_configuration(q)
uid = self.select_target() uid = self.select_target()
bbox = self.get_target_bbox(uid) bbox = self.get_target_bbox(uid)
return bbox return bbox
def set_initial_arm_configuration(self): def set_arm_configuration(self, q):
q = [
self.rng.uniform(-0.17, 0.17), # 0.0
self.rng.uniform(-0.96, -0.62), # -0.79,
self.rng.uniform(-0.17, 0.17), # 0.0
self.rng.uniform(-2.36, -2.19), # -2.36,
0.0,
self.rng.uniform(1.57, 1.91), # 1.57
self.rng.uniform(0.62, 0.96), # 0.79,
]
for i, q_i in enumerate(q): for i, q_i in enumerate(q):
p.resetJointState(self.arm.uid, i, q_i, 0) p.resetJointState(self.arm.uid, i, q_i, 0)
p.resetJointState(self.arm.uid, 9, 0.04, 0) p.resetJointState(self.arm.uid, 9, 0.04, 0)
@ -86,7 +78,6 @@ class Simulation:
class Scene: class Scene:
def __init__(self): def __init__(self):
self.urdfs_path = Path(rospack.get_path("vgn")) / "assets/urdfs" self.urdfs_path = Path(rospack.get_path("vgn")) / "assets/urdfs"
self.origin = None
self.support_uid = -1 self.support_uid = -1
self.object_uids = [] self.object_uids = []
@ -130,7 +121,8 @@ def find_urdfs(root):
class RandomScene(Scene): class RandomScene(Scene):
def __init__(self): def __init__(self):
super().__init__() super().__init__()
self.center = np.r_[[0.5, 0.0, 0.2]] # self.center = np.r_[0.5, 0.0, 0.2]
self.center = np.r_[0.6, 0.0, 0.1]
self.length = 0.3 self.length = 0.3
self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0] self.origin = self.center - np.r_[0.5 * self.length, 0.5 * self.length, 0.0]
self.object_urdfs = find_urdfs(self.urdfs_path / "packed" / "test") self.object_urdfs = find_urdfs(self.urdfs_path / "packed" / "test")
@ -139,7 +131,7 @@ class RandomScene(Scene):
def load(self, rng, attempts=10): def load(self, rng, attempts=10):
self.load_support(self.support_urdf, Rotation.identity(), self.center, 0.3) self.load_support(self.support_urdf, Rotation.identity(), self.center, 0.3)
urdfs, scale = rng.choice(self.object_urdfs, 4), 0.8 urdfs, scale = rng.choice(self.object_urdfs, 5), 0.8
for urdf in urdfs: for urdf in urdfs:
uid = self.load_object(urdf, Rotation.identity(), np.zeros(3), scale) uid = self.load_object(urdf, Rotation.identity(), np.zeros(3), scale)
lower, upper = p.getAABB(uid) lower, upper = p.getAABB(uid)
@ -159,6 +151,12 @@ class RandomScene(Scene):
# No placement found, remove the object # No placement found, remove the object
self.remove_object(uid) self.remove_object(uid)
def sample_initial_configuration(self, rng):
# q = [0.0, -0.79, 0.0, -2.36, 0.0, 1.57, 0.79]
q = [0.0, -0.96, 0.0, -2.09, 0.0, 1.66, 0.79]
q += rng.uniform(-0.08, 0.08, 7)
return q
scenes = { scenes = {
"random": RandomScene, "random": RandomScene,