From 2f07ceab82db0381bcb55e6391394615cd58538b Mon Sep 17 00:00:00 2001 From: Michel Breyer Date: Mon, 12 Jul 2021 14:52:05 +0200 Subject: [PATCH] Tune initial configuration --- active_grasp/simulation.py | 15 +++++++++++---- launch/active_grasp.yaml | 1 + 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/active_grasp/simulation.py b/active_grasp/simulation.py index d1c7d72..9e2ad9b 100644 --- a/active_grasp/simulation.py +++ b/active_grasp/simulation.py @@ -1,6 +1,7 @@ from pathlib import Path import pybullet as p import rospkg +import time from active_grasp.utils import * from robot_utils.bullet import * @@ -34,7 +35,7 @@ class Simulation(BtSim): ori = Rotation.from_rotvec(np.array([0, 0, np.pi / 2])).as_quat() p.loadURDF("table/table.urdf", baseOrientation=ori, useFixedBase=True) self.length = 0.3 - self.origin = [-0.35, -0.5 * self.length, 0.5] + self.origin = [-0.3, -0.5 * self.length, 0.5] def load_robot(self): self.T_W_B = Transform(Rotation.identity(), np.r_[-0.6, 0.0, 0.4]) @@ -54,9 +55,15 @@ class Simulation(BtSim): return self.get_target_bbox(uid) def set_initial_arm_configuration(self): - q = self.arm.configurations["ready"] - q[0] = np.deg2rad(self.rng.uniform(-10, 10)) - q[5] = np.deg2rad(self.rng.uniform(90, 105)) + 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): p.resetJointState(self.arm.uid, i, q_i, 0) p.resetJointState(self.arm.uid, 9, 0.04, 0) diff --git a/launch/active_grasp.yaml b/launch/active_grasp.yaml index 1bd7e0f..75ae77a 100644 --- a/launch/active_grasp.yaml +++ b/launch/active_grasp.yaml @@ -12,6 +12,7 @@ active_grasp: frame_id: cam_optical_frame info_topic: /cam/depth/camera_info depth_topic: /cam/depth/image_raw + vgn: model: $(find vgn)/assets/models/vgn_conv.pth finger_depth: 0.05