def main(cfg): # Initialize digits digits = tacto.Sensor(**cfg.tacto) # Initialize World log.info("Initializing world") px.init() p.resetDebugVisualizerCamera(**cfg.pybullet_camera) robot = SawyerGripper(**cfg.sawyer_gripper) # [21, 24] digits.add_camera(robot.id, robot.digit_links) # Add object to pybullet and digit simulator obj = px.Body(**cfg.object) digits.add_body(obj) np.set_printoptions(suppress=True) # run p.stepSimulation in another thread t = px.utils.SimulationThread(real_time_factor=1.0) t.start() robot.reset() panel = px.gui.RobotControlPanel(robot) panel.start() while True: color, depth = digits.render() digits.updateGUI(color, depth) time.sleep(0.1)
def __init__(self, cfg_path='template_project.yaml', use_visualizer=False, name="TemplateEnv"): """Initialize the environment. Set up any variables that are necessary for the environment and your task. """ #NOTE: Tacto does not seem to be registering the peg even though it is making contact in PyBullet super().__init__(cfg_path, use_visualizer, name) self.digits = tacto.Sensor(**self.config["tacto"]) self.goal_position = self.robot_interface.ee_position self.object_interface = self.world.object_interfaces['peg'] self.update_goal_position() self.robot_interface.reset() self.reset_position = self.robot_interface.ee_position self._initial_ee_orn = self.robot_interface.ee_orientation left_joint = self.robot_interface.get_joint_id_from_name('joint_finger_tip_left') right_joint = self.robot_interface.get_joint_id_from_name('joint_finger_tip_right') self.digits.add_camera(self.robot_interface.arm_id, [left_joint, right_joint]) self.tacto_add_objects() self.CONV_RADIUS = 0.05 self.in_position = False self.grasped = False
def test_rendering_fps(benchmark): sensor = tacto.Sensor(width=120, height=160, visualize_gui=True) px.init_pybullet(mode=p.DIRECT) digit = px.Body( "../meshes/digit.urdf", base_orientation=[0.0, -0.707106, 0.0, 0.707106], ) obj = px.Body( "../examples/objects/sphere_small.urdf", base_position=[-0.015, 0, 0.035], global_scaling=0.15, ) sensor.add_camera(digit.id, [-1]) sensor.add_body(obj) # step the simulation till the ball falls on the digit camera for i in range(10): p.stepSimulation() # Use pytest-benchmark to benchmark the rendering performance # run 1000 times, the reported number will be millisecond instead of seconds benchmark(render, sensor) # visualize the resulting image to make sure it's not empty colors, depths = sensor.render() return colors[0]
def main(cfg): # Initialize digits bg = cv2.imread("conf/bg_digit_240_320.jpg") digits = tacto.Sensor(**cfg.tacto, background=bg) # Initialize World log.info("Initializing world") px.init() p.resetDebugVisualizerCamera(**cfg.pybullet_camera) # Create and initialize DIGIT digit_body = px.Body(**cfg.digit) digits.add_camera(digit_body.id, [-1]) # Add object to pybullet and tacto simulator obj = px.Body(**cfg.object) digits.add_body(obj) # Create control panel to control the 6DoF pose of the object panel = px.gui.PoseControlPanel(obj, **cfg.object_control_panel) panel.start() log.info( "Use the slides to move the object until in contact with the DIGIT") # run p.stepSimulation in another thread t = px.utils.SimulationThread(real_time_factor=1.0) t.start() while True: color, depth = digits.render() digits.updateGUI(color, depth) t.stop()
def load_objects(self): # Initialize digit and robot self.digits = tacto.Sensor(**self.cfg.tacto) if "env" not in self.cfg.sawyer_gripper.robot_params.urdf_path: robot_urdf_path = "env/" + self.cfg.sawyer_gripper.robot_params.urdf_path self.cfg.sawyer_gripper.robot_params.urdf_path = pkg_path(robot_urdf_path) self.robot = SawyerGripper(**self.cfg.sawyer_gripper, physics_client=self.physics_client) self.digits.add_camera(self.robot.id, self.robot.digit_links) # Load objects self.table = px.Body(**self.cfg.objects.table, physics_client=self.physics_client) self.load_board_and_peg()
def __init__(self, config_path=_get_default_config_path()): px.init(mode=p.GUI) self.cfg = OmegaConf.load(config_path) self.robot = SawyerGripper(**self.cfg.sawyer_gripper) self.obj = px.Body(**self.cfg.object) self.digits = tacto.Sensor(**self.cfg.tacto) self.camera = Camera() self.viewer = None self.digits.add_camera(self.robot.id, self.robot.digit_links) self.digits.add_body(self.obj) self.reset()
def __init__(self, cfg_path=None, use_visualizer=False, name=None): """Initialize. Parameters ---------- config: dict A dict with config parameters arena: container for setting up the world in which robot and objects interact use_visualizer: Whether or not to use visualizer. NOTE: Pybullet only allows for one simulation to be connected to GUI. It is up to the user to manage this """ super().__init__(cfg_path, use_visualizer, name) self.goal_position = self.robot_interface.ee_position self.digits = tacto.Sensor(**self.config["tacto"]) # for sim we are tracking an object, increase goal position to be above # the actual position of the object. left_joint = self.robot_interface.get_joint_id_from_name( 'joint_finger_tip_left') right_joint = self.robot_interface.get_joint_id_from_name( 'joint_finger_tip_right') #print(f"-----------------{self.robot_interface.get_link_id_from_name('right_hand')}-----------------") #print(f"-----------------LEFT_IDX: {left_joint}-------------") #print(f"-----------------RIGHT_IDX: {right_joint}-------------") self.digits.add_camera(self.robot_interface.arm_id, [left_joint, right_joint]) obj_path = 'data/objects/ycb/013_apple/google_16k/textured_relative.urdf' #os.path.join(self.world.arena.data_dir, 'objects/ycb/013_apple/google_16k/textured.urdf') obj_id = self.world.arena.object_dict['013_apple'] self.digits.add_object(obj_path, obj_id, globalScaling=1.0) self.object_interface = self.world.object_interfaces['013_apple'] self.update_goal_position() self.robot_interface.reset() self.reset_position = self.robot_interface.ee_position self._initial_ee_orn = self.robot_interface.ee_orientation self.is_lowered = False self.gripper_set = False
def __init__(self, cfg_path='template_project.yaml', use_visualizer=False, name="TemplateEnv"): """Initialize the environment. Set up any variables that are necessary for the environment and your task. """ #NOTE: Tacto does not seem to be registering the peg even though it is making contact in PyBullet super().__init__(cfg_path, use_visualizer, name) self.state_list = ["PEG_SETUP", "PEG_GRAB", "PEG_MOVING", "PEG_COMPLETE"] self.state_dict = { "PEG_SETUP": {"method": self._peg_setup_exec, "next": "PEG_GRAB"}, "PEG_GRAB": {"method": self._peg_grab_exec, "next": "PEG_MOVING"}, "PEG_MOVING": {"method": self._peg_move_exec, "next": "PEG_COMPLETE"}, "PEG_COMPLETE": {"method": self._peg_complete_exec, "next": None}, } self.curr_state = self.state_list[0] self.digits = tacto.Sensor(**self.config["tacto"]) self.goal_position = self.robot_interface.ee_position self.peg_interface = self.world.object_interfaces['peg'] self.hole_interface = self.world.object_interfaces['hole_box'] self.term_state = None#"PEG_COMPLETE" #NOTE: We terminate at grab for now self.robot_interface.reset() self.reset_position = self.robot_interface.ee_position self._initial_ee_orn = self.robot_interface.ee_orientation left_joint = self.robot_interface.get_joint_id_from_name('joint_finger_tip_left') right_joint = self.robot_interface.get_joint_id_from_name('joint_finger_tip_right') self.digits.add_camera(self.robot_interface.arm_id, [left_joint, right_joint]) self.tacto_add_objects() self.CONV_RADIUS = 0.05 self.in_position = False self.grasped = False
def __init__(self, cfg_path='template_project.yaml', use_visualizer=False, name="TemplateEnv"): """Initialize the environment. Set up any variables that are necessary for the environment and your task. """ super().__init__(cfg_path, use_visualizer, name) self.digits = tacto.Sensor(**self.config["tacto"]) #left_joint = self.robot_interface.get_link_id_from_name('finger_left_tip') #right_joint = self.robot_interface.get_link_id_from_name('finger_right_tip') left_joint = self.robot_interface.get_joint_id_from_name( 'joint_finger_tip_left') right_joint = self.robot_interface.get_joint_id_from_name( 'joint_finger_tip_right') print( f"-----------------{self.robot_interface.get_link_id_from_name('right_hand')}-----------------" ) print(f"-----------------LEFT_IDX: {left_joint}-------------") print(f"-----------------RIGHT_IDX: {right_joint}-------------") self.digits.add_camera(self.robot_interface.arm_id, [left_joint, right_joint]) obj_path = 'data/objects/ycb/013_apple/google_16k/textured_relative.urdf' #os.path.join(self.world.arena.data_dir, 'objects/ycb/013_apple/google_16k/textured.urdf') obj_id = self.world.arena.object_dict['013_apple'] self.digits.add_object(obj_path, obj_id, globalScaling=1.0) self.robot_interface.set_gripper_to_value(0.1) self.goal_position = self.robot_interface.ee_position self.object_interface = self.world.object_interfaces['013_apple'] self.update_goal_position() self.robot_interface.reset() self.reset_position = self.robot_interface.ee_position self._initial_ee_orn = self.robot_interface.ee_orientation
def main(cfg): # Initialize OmniTact omnitact = tacto.Sensor( **cfg.tacto, **{"config_path": tacto.get_omnitact_config_path()}) # Initialize World log.info("Initializing world") px.init() p.resetDebugVisualizerCamera(**cfg.pybullet_camera) p.configureDebugVisualizer( p.COV_ENABLE_SHADOWS, 1, lightPosition=[50, 0, 80], ) # Create and initialize OmniTact body = px.Body(**cfg.omnitact) omnitact.add_camera(body.id, [-1]) obj = px.Body(**cfg.object) omnitact.add_body(obj) # Create control panel to control the 6DoF pose of the object panel = px.gui.PoseControlPanel(obj, **cfg.object_control_panel) panel.start() log.info( "Use the slides to move the object until in contact with the OmniTact") # run p.stepSimulation in another thread t = px.utils.SimulationThread(real_time_factor=1.0) t.start() while True: color, depth = omnitact.render() omnitact.updateGUI(color, depth) t.stop()
def main(cfg): # Initialize digits # FIXME(poweic): this has to be done before p.connect if PYOPENGL_PLATFORM set to "egl" digits = tacto.Sensor(**cfg.tacto) # Initialize World log.info("Initializing world") px.init() p.resetDebugVisualizerCamera(**cfg.pybullet_camera) # Add allegro hand allegro = px.Body(**cfg.allegro) # Add cameras to tacto simulator digits.add_camera(allegro.id, cfg.digit_link_id_allegro) # Add object to pybullet and tacto simulator obj = px.Body(**cfg.object) digits.add_body(obj) # Create control panel to control the 6DoF pose of the object panel = px.gui.PoseControlPanel(obj, **cfg.object_control_panel) panel.start() log.info( "Use the slides to move the object until in contact with the DIGIT") # run p.stepSimulation in another thread t = px.utils.SimulationThread(real_time_factor=1.0) t.start() while True: color, depth = digits.render() digits.updateGUI(color, depth) t.stop()
def __init__(self, cfg_path='template_project.yaml', use_visualizer=False, name="TemplateEnv"): """Initialize the environment. Set up any variables that are necessary for the environment and your task. """ super().__init__(cfg_path, use_visualizer, name) #Movement State Machine Setup self.state_list = [ "PEG_SETUP", "PEG_GRAB", "PEG_MOVING", "PEG_COMPLETE" ] self.state_dict = { "PEG_SETUP": { "method": self._peg_setup_exec, "next": "PEG_GRAB" }, "PEG_GRAB": { "method": self._peg_grab_exec, "next": None }, } self.peg_interface = self.world.object_interfaces['peg'] self.hole_interface = self.world.object_interfaces['hole_box'] self.term_state = None #"PEG_MOVING" #None #"PEG_COMPLETE" #Object Information self.BOX_W = 1.0 self.BOX_H = 0.2 self.HOLE_W = 0.2 self.PEG_H = 0.3 self.PEG_W = 0.2 self.scale_dict = {} #Tacto Setup self.digits = tacto.Sensor(**self.config["tacto"]) self.left_joint_idx = self.robot_interface.get_joint_id_from_name( 'joint_finger_tip_left') self.right_joint_idx = self.robot_interface.get_joint_id_from_name( 'joint_finger_tip_right') self.left_finger_link = self.robot_interface.get_link_id_from_name( 'finger_left_tip') self.right_finger_link = self.robot_interface.get_link_id_from_name( 'finger_right_tip') self.gripper_base_link = self.robot_interface.get_link_id_from_name( 'gripper_base_link') self.digits.add_camera(self.robot_interface.arm_id, [self.left_joint_idx, self.right_joint_idx]) self.focus_point_link = self.robot_interface.get_link_id_from_name( 'focus_point') self.ee_point = self.focus_point_link self.CONV_RADIUS = 0.05 self.PLANAR_RADIUS = self.CONV_RADIUS self.GOAL_RADIUS = self.PLANAR_RADIUS * 0.1 self.FAIL_RADIUS = 0.2 self.PEG_GRIP_MAX_DIST = 0.15 self.tacto_add_objects() #Robot Setup self.reset_position = self.robot_interface.ee_position self._initial_ee_orn = self.robot_interface.ee_orientation #Peg and hole initial_positions self.peg_initial_pos = [0, 0, 0] self.hole_initial_pos = [0, 0, 0] self.observation_space = spaces.Box(np.full((FEATURE_LEN), -np.inf), np.full((FEATURE_LEN), np.inf), dtype=np.float32) #Torch Initialization self.device = torch.device("cuda") self.z_dim = 128 self.action_dim = 3 self.model = SensorFusionSelfSupervised( device=self.device, encoder=False, deterministic=False, z_dim=self.z_dim, action_dim=self.action_dim, ).to(self.device) self.model_path = '/home/mason/multimodal_tacto_rep/multimodal/no_tacto_logging/models/weights_itr_49.ckpt' #'/home/mason/multimodal_tacto_rep/multimodal/logging/202105312206_/models/weights_itr_48.ckpt' self.load_model(self.model_path)
def create_scene(self): """ Create scene and tacto simulator """ # Initialize digits digits = tacto.Sensor( width=self.tactoResolution[0], height=self.tactoResolution[1], visualize_gui=self.visTacto, ) if self.visPyBullet: self.physicsClient = pb.connect(pb.GUI) else: self.physicsClient = pb.connect(pb.DIRECT) pb.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally pb.setGravity(0, 0, -9.81) # Major Tom to planet Earth # Set camera view pb.resetDebugVisualizerCamera( cameraDistance=0.12, cameraYaw=0, cameraPitch=-20, cameraTargetPosition=[0, 0, 0.02], ) pb.loadURDF("plane.urdf") # Create plane digitURDF = "setup/sensors/digit.urdf" # Set upper digit digitPos1 = [0, 0, 0.011] digitOrn1 = pb.getQuaternionFromEuler([0, -np.pi / 2, 0]) digitID1 = pb.loadURDF( digitURDF, basePosition=digitPos1, baseOrientation=digitOrn1, useFixedBase=True, ) digits.add_camera(digitID1, [-1]) # Set lower digit digitPos2 = [0, 0, 0.07] digitOrn2 = pb.getQuaternionFromEuler([0, np.pi / 2, np.pi]) digitID2 = pb.loadURDF( digitURDF, basePosition=digitPos2, baseOrientation=digitOrn2, ) digits.add_camera(digitID2, [-1]) # Create object and GUI controls init_xyz = np.array([0, 0.0, 8]) # Add object to pybullet and tacto simulator urdfObj = "setup/objects/sphere_small.urdf" objPos = np.array([-1.5, 0, 4]) / 100 objOrn = pb.getQuaternionFromEuler([0, 0, 0]) globalScaling = 0.15 # Add ball urdf into pybullet and tacto objId = digits.loadURDF(urdfObj, objPos, objOrn, globalScaling=globalScaling) # Add constraint to movable digit (upper) cid = pb.createConstraint(digitID2, -1, -1, -1, pb.JOINT_FIXED, [0, 0, 0], [0, 0, 0], init_xyz / 100) # Save variables self.digits = digits self.digitID1, self.digitPos1, self.digitOrn1 = digitID1, digitPos1, digitOrn1 self.digitID2, self.digitPos2, self.digitOrn2 = digitID2, digitPos2, digitOrn2 self.objId, self.objPos, self.objOrn = objId, objPos, objOrn self.cid = cid
dd.io.save(outputFn, newData[k]) self.dataList = [] self.id += 1 log = Log("data/grasp") # Initialize World logging.info("Initializing world") physicsClient = pb.connect(pb.DIRECT) pb.setAdditionalSearchPath(pybullet_data.getDataPath()) # optionally pb.setGravity(0, 0, -9.81) # Major Tom to planet Earth # Initialize digits digits = tacto.Sensor(width=240, height=320, visualize_gui=False) pb.resetDebugVisualizerCamera( cameraDistance=0.6, cameraYaw=15, cameraPitch=-20, # cameraTargetPosition=[-1.20, 0.69, -0.77], cameraTargetPosition=[0.5, 0, 0.08], ) planeId = pb.loadURDF("plane.urdf") # Create plane robotURDF = "setup/robots/sawyer_wsg50.urdf" # robotURDF = "robots/wsg50.urdf" robotID = pb.loadURDF(robotURDF, useFixedBase=True) rob = Robot(robotID)
def __init__(self, cfg_path='template_project.yaml', use_visualizer=False, name="TemplateEnv"): """Initialize the environment. Set up any variables that are necessary for the environment and your task. """ super().__init__(cfg_path, use_visualizer, name) #Movement State Machine Setup self.state_list = [ "PEG_SETUP", "PEG_GRAB", "PEG_MOVING", "PEG_COMPLETE" ] self.state_dict = { "PEG_SETUP": { "method": self._peg_setup_exec, "next": "PEG_GRAB" }, "PEG_GRAB": { "method": self._peg_grab_exec, "next": "PEG_MOVING" }, "PEG_MOVING": { "method": self._random_peg_move_exec, "next": "PEG_COMPLETE" }, } self.peg_interface = self.world.object_interfaces['peg'] self.hole_interface = self.world.object_interfaces['hole_box'] self.term_state = None #"PEG_MOVING" #None #"PEG_COMPLETE" #Object Information self.BOX_W = 1.0 self.BOX_H = 0.2 self.HOLE_W = 0.2 self.PEG_H = 0.3 self.PEG_W = 0.2 self.scale_dict = {} #Tacto Setup self.digits = tacto.Sensor(**self.config["tacto"]) self.left_joint_idx = self.robot_interface.get_joint_id_from_name( 'joint_finger_tip_left') self.right_joint_idx = self.robot_interface.get_joint_id_from_name( 'joint_finger_tip_right') self.left_finger_link = self.robot_interface.get_link_id_from_name( 'finger_left_tip') self.right_finger_link = self.robot_interface.get_link_id_from_name( 'finger_right_tip') self.gripper_base_link = self.robot_interface.get_link_id_from_name( 'gripper_base_link') self.digits.add_camera(self.robot_interface.arm_id, [self.left_joint_idx, self.right_joint_idx]) self.focus_point_link = self.robot_interface.get_link_id_from_name( 'focus_point') self.ee_point = self.focus_point_link self.CONV_RADIUS = 0.05 self.tacto_add_objects() #Robot Setup self.reset_position = self.robot_interface.ee_position self._initial_ee_orn = self.robot_interface.ee_orientation #Peg and hole initial_positions self.peg_initial_pos = [0, 0, 0] self.hole_initial_pos = [0, 0, 0]