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]
