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):
self.moveit = MoveItClient("panda_arm")
rospy.sleep(1.0) # wait for connections to be established
table_height = 0.22
msg = to_pose_stamped_msg(Transform.t([0.4, 0, table_height]), self.base_frame)
msg = to_pose_stamped_msg(Transform.t([0.4, 0, 0.4]), self.base_frame)
self.moveit.scene.add_box("table", msg, size=(0.5, 0.5, 0.02))
def switch_to_cartesian_velocity_control(self):

26
active_grasp/simulation.py
View File

@ -46,21 +46,13 @@ class Simulation:
def reset(self):
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()
bbox = self.get_target_bbox(uid)
return bbox
def set_initial_arm_configuration(self):
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,
]
def set_arm_configuration(self, q):
for i, q_i in enumerate(q):
p.resetJointState(self.arm.uid, i, q_i, 0)
p.resetJointState(self.arm.uid, 9, 0.04, 0)
@ -86,7 +78,6 @@ class Simulation:
class Scene:
def __init__(self):
self.urdfs_path = Path(rospack.get_path("vgn")) / "assets/urdfs"
self.origin = None
self.support_uid = -1
self.object_uids = []
@ -130,7 +121,8 @@ def find_urdfs(root):
class RandomScene(Scene):
def __init__(self):
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.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")
@ -139,7 +131,7 @@ class RandomScene(Scene):
def load(self, rng, attempts=10):
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:
uid = self.load_object(urdf, Rotation.identity(), np.zeros(3), scale)
lower, upper = p.getAABB(uid)
@ -159,6 +151,12 @@ class RandomScene(Scene):
# No placement found, remove the object
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 = {
"random": RandomScene,