def create_robot(request):
robot_name = request.config.getoption('robot_name')
if request.config.getoption('sim_env') == 'gazebo':
return Robot(robot_name, pb=False, use_cam=False)
elif request.config.getoption('sim_env') == 'pybullet':
return Robot(robot_name,
pb=True,
use_cam=False,
arm_cfg={'render': False})
else:
raise ValueError('unknown simulation environment')
def main():
"""
This function demonstrates how to setup camera
and get rgb/depth images and segmentation mask.
"""
robot = Robot('ur5e', pb_cfg={'gui': True,
'opengl_render': False})
focus_pt = [0, 0, 1] # ([x, y, z])
robot.cam.setup_camera(focus_pt=focus_pt,
dist=3,
yaw=90,
pitch=0,
roll=0)
robot.arm.go_home()
rgb, depth, seg = robot.cam.get_images(get_rgb=True,
get_depth=True,
get_seg=True)
plt.figure()
plt.imshow(rgb)
plt.figure()
plt.imshow(depth * 25, cmap='gray', vmin=0, vmax=255)
log_info('Maximum Depth (m): %f' % np.max(depth))
log_info('Minimum Depth (m): %f' % np.min(depth))
plt.figure()
plt.imshow(seg)
plt.show()
def main():
"""
This function demonstrates how to properly save depth
images in python.
"""
robot = Robot('ur5e', pb_cfg={'gui': False, 'opengl_render': False})
focus_pt = [0, 0, 1]
robot.cam.setup_camera(focus_pt=focus_pt, dist=3, yaw=90, pitch=0, roll=0)
robot.arm.go_home()
rgb, depth, seg = robot.cam.get_images(get_rgb=True,
get_depth=True,
get_seg=True)
log_info(f'Depth image min:{depth.min()} m, max: {depth.max()} m.')
# we first convert the depth image into CV_16U format.
# The maximum value for CV_16U is 65536.
# Since png files only keep the integer values, we need to scale the
# depth images to keep enough resolution.
scale = 1000.0
sdepth = depth * scale
img_name = 'depth.png'
# by converting the image data to CV_16U, the resolution of the
# depth image data becomes 1 / scale m (0.001m in this case).
cv2.imwrite(img_name, sdepth.astype(np.uint16))
re_depth = cv2.imread(img_name, cv2.IMREAD_UNCHANGED)
sre_depth = re_depth / scale
log_info(f'Saved depth image min:{sre_depth.min()} m, '
f'max: {sre_depth.max()} m.')
def __init__(self, action_repeat=10, gui=True):
self._action_repeat = action_repeat
self.robot = Robot('ur5e_2f140',
pb_cfg={
'gui': gui,
'realtime': False
})
self._ee_pos_scale = 0.02
self._ee_ori_scale = np.pi / 36.0
self.cams = []
pb_client = self.robot.pb_client
for i in range(2):
self.cams.append(
RGBDCameraPybullet(cfgs=self._camera_cfgs(),
pb_client=pb_client))
self._setup_cameras()
self.reset()
self._action_bound = 1.0
self._action_high = np.array([self._action_bound] * 5)
self.action_space = spaces.Box(low=-self._action_high,
high=self._action_high,
dtype=np.float32)
ob = self._get_obs()
self.observation_space = spaces.Box(low=0,
high=255,
shape=ob.shape,
dtype=np.uint8)
def main():
"""
Visualize the point cloud from the RGBD camera.
"""
robot = Robot('ur5e')
robot.arm.go_home()
robot.cam.setup_camera(focus_pt=robot.arm.robot_base_pos,
dist=3,
yaw=55,
pitch=-30,
roll=0)
depth_max = 5.0
vis = open3d.visualization.Visualizer()
vis.create_window("Point Cloud")
pcd = open3d.geometry.PointCloud()
pts, colors = robot.cam.get_pcd(in_world=True,
filter_depth=True,
depth_max=depth_max)
pcd.points = open3d.utility.Vector3dVector(pts)
pcd.colors = open3d.utility.Vector3dVector(colors / 255.0)
vis.add_geometry(pcd)
while True:
pts, colors = robot.cam.get_pcd(in_world=True,
filter_depth=True,
depth_max=depth_max)
pcd.points = open3d.utility.Vector3dVector(pts)
pcd.colors = open3d.utility.Vector3dVector(colors / 255.0)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
time.sleep(0.1)
def __init__(self, action_repeat=10, render=False):
self._action_repeat = action_repeat
self.robot = Robot('ur5e_stick', pb=True, pb_cfg={'gui': render, 'realtime':False})
self.ee_ori = [-np.sqrt(2) / 2, np.sqrt(2) / 2, 0, 0]
self._action_bound = 1.0
self._ee_pos_scale = 0.02
self._ee_ori_scale = np.pi / 36.0
self._action_high = np.array([self._action_bound] * 2)
self.action_space = spaces.Box(low=-self._action_high,
high=self._action_high,
dtype=np.float32)
self.goal = np.array([0.75, -0.3, 1.0])
self.init = np.array([0.5, 0.1, 1.0])
self.robot.arm.reset()
ori = euler2quat([0, 0, np.pi / 2])
self.table_id = self.robot.pb_client.load_urdf('table/table.urdf',
[.5, 0, 0.4],
ori,
scaling=0.9)
self.marker_id = self.robot.pb_client.load_geom('box', size=0.05, mass=1,
base_pos=self.goal.tolist(),
rgba=[0, 1, 0, 0.4])
client_id = self.robot.pb_client.get_client_id()
p.setCollisionFilterGroupMask(self.marker_id, -1, 0, 0, physicsClientId=client_id)
p.setCollisionFilterPair(self.marker_id, self.table_id, -1, -1, 1, physicsClientId=client_id)
self.reset()
state_low = np.full(len(self._get_obs()), -float('inf'))
state_high = np.full(len(self._get_obs()), float('inf'))
self.observation_space = spaces.Box(state_low,
state_high,
dtype=np.float32)
def main():
"""
This function demonstrates how the yaw angle (
the yaw angle that is defined in robot.setup_camera) changes
the camera view.
"""
robot = Robot('ur5e')
focus_pt = [0, 0, 1] # ([x, y, z])
robot.arm.go_home()
img = np.random.rand(480, 640)
image = plt.imshow(img, interpolation='none',
animated=True, label="cam")
ax = plt.gca()
while True:
for yaw in range(0, 360, 10):
robot.cam.setup_camera(focus_pt=focus_pt,
dist=3,
yaw=yaw,
pitch=0,
roll=0)
rgb, depth = robot.cam.get_images(get_rgb=True,
get_depth=True)
image.set_data(rgb)
ax.plot([0])
plt.pause(0.01)
def main():
"""
This function demonstrates how to load different kinds of
objects and get state information of objects.
"""
np.set_printoptions(precision=3, suppress=True)
robot = Robot('ur5e_stick')
robot.arm.go_home()
ori = euler2quat([0, 0, np.pi / 2])
robot.pb_client.load_urdf('table/table.urdf',
[1, 0, 0.4],
ori,
scaling=0.9)
sphere_id = robot.pb_client.load_geom('sphere',
size=0.05,
mass=1,
base_pos=[1, 0, 1.0],
rgba=[0, 1, 0, 1])
robot.pb_client.load_geom('box',
size=0.05,
mass=1,
base_pos=[1, 0.12, 1.0],
rgba=[1, 0, 0, 1])
robot.pb_client.load_geom('box',
size=[0.06, 0.02, 0.03],
mass=1,
base_pos=[1.3, 0.12, 1.0],
rgba=[0, 0, 1, 1])
cylinder_id = robot.pb_client.load_geom('cylinder',
size=[0.06, 0.08],
mass=1,
base_pos=[0.8, -0.12, 1.0],
rgba=[0, 1, 1, 1])
duck_id = robot.pb_client.load_geom('mesh',
mass=1,
visualfile='duck.obj',
mesh_scale=0.1,
base_pos=[0.9, -0.4, 1.0],
rgba=[0.5, 0.2, 1, 1])
pos, quat, lin_vel, ang_vel = robot.pb_client.get_body_state(cylinder_id)
log_info('Cylinder:')
log_info(' position: %s' % np.array2string(pos,
precision=2))
log_info(' quaternion: %s' % np.array2string(quat,
precision=2))
log_info(' linear vel: %s' % np.array2string(lin_vel,
precision=2))
log_info(' angular vel: %s' % np.array2string(ang_vel,
precision=2))
log_info('Removing sphere')
robot.pb_client.remove_body(sphere_id)
time.sleep(2)
log_info('Reset duck')
robot.pb_client.reset_body(duck_id, base_pos=[0.9, -0.4, 1.0],
base_quat=[0, 0, 0, 1],
lin_vel=[0, 2, 0],
ang_vel=[0, 0, 2])
time.sleep(10)
def main():
"""
Move the robot end effector in a straight line.
"""
robot = Robot('ur5e')
robot.arm.go_home()
robot.arm.move_ee_xyz([0.1, 0.1, 0.1])
time.sleep(3)
def __init__(self, ifRender=False):
# np.set_printoptions(precision=3, suppress=True)
# table scaling and table center location
self.table_scaling = 0.6 # tabletop x ~ (0.3, 0.9); y ~ (-0.45, 0.45)
self.table_x = 0.6
self.table_y = 0
self.table_z = 0.6
self.table_surface_height = 0.975 # get from running robot.cam.get_pix.3dpt
self.table_ori = euler2quat([0, 0, np.pi / 2])
# task space x ~ (0.4, 0.8); y ~ (-0.3, 0.3)
self.max_arm_reach = 0.91
self.workspace_max_x = 0.75 # 0.8 discouraged, as box can move past max arm reach
self.workspace_min_x = 0.4
self.workspace_max_y = 0.3
self.workspace_min_y = -0.3
# robot end-effector
self.ee_min_height = 0.99
self.ee_rest_height = 1.1 # stick scale="0.0001 0.0001 0.0007"
self.ee_home = [self.table_x, self.table_y, self.ee_rest_height]
# initial object location
self.box_z = 1 - 0.005
self.box_pos = [self.table_x, self.table_y, self.box_z]
self.box_size = 0.02 # distance between center frame and size, box size 0.04
# push config: push_len by default [0.06-0.1]
self.push_len_min = 0.06 # 0.06 ensures no contact with box empiracally
self.push_len_range = 0.04
# image processing config
self.row_min = 40
self.row_max = 360
self.col_min = 0
self.col_max = 640
self.output_row = 100
self.output_col = 200
self.row_scale = (self.row_max - self.row_min) / float(self.output_row)
self.col_scale = (self.col_max - self.col_min) / float(self.output_col)
assert self.col_scale == self.row_scale
# load robot
self.robot = Robot('ur5e_stick', pb=True, pb_cfg={'gui': ifRender})
self.robot.arm.go_home()
self.ee_origin = self.robot.arm.get_ee_pose()
self.go_home()
self._home_jpos = self.robot.arm.get_jpos()
# load table
self.table_id = self.load_table()
# load box
self.box_id = self.load_box()
# initialize camera matrices
self.robot.cam.setup_camera(focus_pt=[0.7, 0, 1.],
dist=0.5,
yaw=90,
pitch=-60,
roll=0)
self.ext_mat = self.robot.cam.get_cam_ext()
self.int_mat = self.robot.cam.get_cam_int()
def main():
"""
Move the robot end effector in a straight line.
"""
robot = Robot('yumi_grippers')
robot.arm.go_home()
robot.arm.right_arm.move_ee_xyz([0.1, 0.1, 0.1])
time.sleep(3)
robot.arm.move_ee_xyz([0.1, -0.1, 0.1], arm='left')
time.sleep(3)
def main():
"""
This function demonstrates how to move the robot arm
to the desired joint positions.
"""
robot = Robot('ur5e')
robot.arm.go_home()
robot.arm.set_jpos([-0.8, -1.2, -2.2, -1.5, 2.0, 0])
# sleep statement is not necessary
time.sleep(3)
robot.arm.set_jpos(0.5, 'shoulder_pan_joint')
time.sleep(3)
def main():
"""
This function demonstrates how to move the robot arm
to the desired joint positions.
"""
dir_path = os.path.dirname(os.path.realpath(__file__))
texture_path = os.path.join(dir_path, 'textures')
robot = Robot('ur5e')
robot.arm.go_home()
modder = TextureModder(pb_client_id=robot.pb_client.get_client_id())
ori = euler2quat([0, 0, np.pi / 2])
table_id = robot.pb_client.load_urdf('table/table.urdf',
[1, 0, 0.4],
ori,
scaling=0.9)
sphere_id = robot.pb_client.load_geom('sphere',
size=0.05,
mass=1,
base_pos=[1, 0, 1.0],
rgba=[0, 1, 0, 1])
box_id = robot.pb_client.load_geom('box',
size=0.05,
mass=1,
base_pos=[1, 0.12, 1.0],
rgba=[1, 0, 0, 1])
duck_id = robot.pb_client.load_geom('mesh',
mass=1,
visualfile='duck.obj',
mesh_scale=0.1,
base_pos=[0.9, -0.4, 1.0],
rgba=[0.5, 0.2, 1, 1])
modder.set_texture(table_id, -1, os.path.join(texture_path, '1.jpg'))
modder.set_texture(sphere_id, -1, os.path.join(texture_path, '2.jpg'))
modder.set_texture(box_id, -1, os.path.join(texture_path, '3.jpg'))
modder.set_texture(duck_id, -1, os.path.join(texture_path, '4.jpg'))
modder.set_texture_path(texture_path)
while True:
start = time.time()
# modder.randomize('rgb')
# modder.randomize('gradient')
# modder.randomize('noise')
# modder.randomize('texture', exclude={robot.arm.robot_id: []})
# modder.randomize('texture',
# exclude={robot.arm.robot_id: [3, 4, 5, 6]})
modder.randomize('all')
print('Time cost (s): ', time.time() - start)
time.sleep(1)
def main():
"""
This function shows an example of block stacking.
"""
np.set_printoptions(precision=4, suppress=True)
robot = Robot('franka')
success = robot.arm.go_home()
if not success:
log_warn('Robot go_home failed!!!')
ori = euler2quat([0, 0, np.pi / 2])
robot.pb_client.load_urdf('table/table.urdf',
[.6, 0, 0.4],
ori,
scaling=0.9)
box_size = 0.03
box_id1 = robot.pb_client.load_geom('box', size=box_size,
mass=0.1,
base_pos=[.5, 0.12, 1.0],
rgba=[1, 0, 0, 1])
box_id2 = robot.pb_client.load_geom('box',
size=box_size,
mass=0.1,
base_pos=[0.3, 0.12, 1.0],
rgba=[0, 0, 1, 1])
robot.arm.eetool.open()
obj_pos = robot.pb_client.get_body_state(box_id1)[0]
move_dir = obj_pos - robot.arm.get_ee_pose()[0]
move_dir[2] = 0
eef_step = 0.025
# an example of using IK with nullspace enabled
ik_kwargs = dict(ns=True)
robot.arm.move_ee_xyz(move_dir, eef_step=eef_step, **dict(ik_kwargs=ik_kwargs))
move_dir = np.zeros(3)
move_dir[2] = obj_pos[2] - robot.arm.get_ee_pose()[0][2]
robot.arm.move_ee_xyz(move_dir, eef_step=eef_step)
robot.arm.eetool.close(wait=False)
robot.arm.move_ee_xyz([0, 0, 0.3], eef_step=eef_step)
obj_pos = robot.pb_client.get_body_state(box_id2)[0]
move_dir = obj_pos - robot.arm.get_ee_pose()[0]
move_dir[2] = 0
robot.arm.move_ee_xyz(move_dir, eef_step=eef_step)
move_dir = obj_pos - robot.arm.get_ee_pose()[0]
move_dir[2] += box_size * 2
robot.arm.move_ee_xyz(move_dir, eef_step=eef_step)
robot.arm.eetool.open()
move_dir[2] = 0.2
robot.arm.move_ee_xyz(move_dir, eef_step=eef_step)
time.sleep(10)
def main():
"""
Move all the joints of the robot in a sine-wave fashion.
"""
robot = Robot('ur5e')
robot.arm.go_home()
A = 0.2
f = 0.4
start_time = time.time()
while True:
elapsed_time = time.time() - start_time
vels = [sin_wave(elapsed_time, f, A)] * robot.arm.arm_dof
robot.arm.set_jvel(vels)
time.sleep(0.01)
def main():
"""
This function demonstrates how to load a custom
end effector to the UR5e robot in pybullet.
NOTE: This implementation using PyBullet's createConstraint
function is known to have issues with fixed joints (fixed joints
are not actually rigidly fixed).
"""
robot = Robot('ur5e',
pb=True,
use_eetool=False,
arm_cfg={'self_collision': False})
robot.arm.go_home()
# custom EE tool can either be added by making a new separate URDF
# or using the load_geom capbility in pb_util file
# uncomment below to load an example custom URDF
root_path = os.path.dirname(os.path.realpath(__file__))
urdf_path = '../../../src/airobot/urdfs/custom_ee_tools/stick_ee_tool.urdf'
stick_id = robot.pb_client.load_urdf(
os.path.join(root_path, urdf_path),
[0.3, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0],
)
# uncomment below to load a basic geometry with load_geom
# from airobot.utils.pb_util import load_geom
# stick_id = load_geom('cylinder', size=[0.015, 0.1524], mass=0.01,
# base_pos=[0.3, 0.0, 0.0], rgba=[0, 1, 1, 1])
robot.pb_client.createConstraint(robot.arm.robot_id,
robot.arm.ee_link_id,
stick_id,
-1,
p.JOINT_FIXED,
jointAxis=[1, 1, 1],
parentFramePosition=[0, 0, 0],
childFramePosition=[0, 0, -0.077],
childFrameOrientation=euler2quat(
[0, 0, 0]))
time.sleep(5)
def main():
"""
Move the robot end effector in a straight line.
The pb=False flag is set because we are using the real robot
(pb -- pybullet).
First movement is executed with MoveIt!, and second movement
is executed using urscript commands.
"""
robot = Robot('ur5e_2f140',
pb=False,
use_cam=False)
robot.arm.go_home()
robot.arm.move_ee_xyz([0.2, 0.0, 0.0], wait=True)
time.sleep(1.0)
robot.arm.set_comm_mode(use_urscript=True)
robot.arm.move_ee_xyz([-0.2, 0.0, 0.0], wait=True)
def __init__(self, action_repeat=10, gui=True):
self._action_repeat = action_repeat
self.robot = Robot('ur5e_2f140',
pb_cfg={'gui': gui,
'realtime': False})
self.ee_ori = [-np.sqrt(2) / 2, np.sqrt(2) / 2, 0, 0]
self._action_bound = 1.0
self._ee_pos_scale = 0.02
self._ee_ori_scale = np.pi / 36.0
self._action_high = np.array([self._action_bound] * 5)
self.action_space = spaces.Box(low=-self._action_high,
high=self._action_high,
dtype=np.float32)
state_low = np.full(len(self._get_obs()), -float('inf'))
state_high = np.full(len(self._get_obs()), float('inf'))
self.observation_space = spaces.Box(state_low,
state_high,
dtype=np.float32)
self.reset()
def main():
"""
Rotate the end effector about a single axis, one at a time
"""
robot = Robot('ur5e_2f140')
robot.arm.go_home()
robot.arm.rot_ee_xyz(np.pi / 4, axis='x')
time.sleep(1)
robot.arm.rot_ee_xyz(-np.pi / 4, axis='x')
time.sleep(3)
robot.arm.rot_ee_xyz(np.pi / 4, axis='y')
time.sleep(1)
robot.arm.rot_ee_xyz(-np.pi / 4, axis='y')
time.sleep(3)
robot.arm.rot_ee_xyz(np.pi / 4, axis='z')
time.sleep(1)
robot.arm.rot_ee_xyz(-np.pi / 4, axis='z')
time.sleep(3)
def main():
"""
3D Point cloud visualization of the current scene.
This is useful to see how does the point cloud that
the pushing script works on look like
"""
parser = argparse.ArgumentParser(description='Argument Parser')
parser.add_argument('--z_min',
type=float,
default=-0.15,
help='minimium acceptable z value')
args = parser.parse_args()
np.set_printoptions(precision=4, suppress=True)
robot = Robot('ur5e_2f140', pb=False, use_cam=True)
pre_jnts = [1.57, -1.66, -1.92, -1.12, 1.57, 0]
robot.arm.set_jpos(pre_jnts)
cam_pos, cam_ori = read_cam_ext('ur5e')
robot.cam.set_cam_ext(cam_pos, cam_ori)
vis = open3d.visualization.Visualizer()
vis.create_window("Point Cloud")
pcd = open3d.geometry.PointCloud()
pts, colors = robot.cam.get_pcd(in_world=True, filter_depth=True)
pts, colors = filter_points(pts, colors, z_lowest=args.z_min)
pcd.points = open3d.utility.Vector3dVector(pts)
pcd.colors = open3d.utility.Vector3dVector(colors / 255.0)
coord = open3d.geometry.TriangleMesh.create_coordinate_frame(1, [0, 0, 0])
vis.add_geometry(coord)
vis.add_geometry(pcd)
while True:
pts, colors = robot.cam.get_pcd(in_world=True, filter_depth=True)
pts, colors = filter_points(pts, colors, z_lowest=args.z_min)
pcd.points = open3d.utility.Vector3dVector(pts)
pcd.colors = open3d.utility.Vector3dVector(colors / 255.0)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
time.sleep(0.1)
def main():
"""
Move the robot end effector to the desired pose.
"""
robot = Robot('ur5e')
robot.arm.go_home()
tgt_pos = [0.45, 0.2, 1.3]
robot.arm.set_ee_pose(tgt_pos)
# sleep statement is not necessary
time.sleep(1)
tgt_pos = [0.6, -0.15, 1.2]
tgt_euler = [1.57, 0.0, 0.0]
robot.arm.set_ee_pose(tgt_pos, tgt_euler)
time.sleep(1)
pos, quat, rot, euler = robot.arm.get_ee_pose()
log_info('End effector pose:')
log_info('Position:')
log_info(pos)
log_info('Euler angles:')
log_info(euler)
def main():
"""
Push the objects on the table randomly.
The robot will first go to a reset position so that the robot
arm does not block the camera's view. And a clustering alogrithm
is used to cluster the 3D point cloud of the objects on the table.
Then the gripper pushs a randomly selected object on the table.
And the gripper goes back to a reset position.
"""
parser = argparse.ArgumentParser(description='Argument Parser')
parser.add_argument('--z_min',
type=float,
default=-0.15,
help='minimium acceptable z value')
args = parser.parse_args()
np.set_printoptions(precision=4, suppress=True)
bot = Robot('ur5e_2f140', pb=False, use_cam=True)
cam_pos, cam_ori = read_cam_ext('ur5e')
bot.cam.set_cam_ext(cam_pos, cam_ori)
bot.arm.go_home()
bot.arm.eetool.activate()
bot.arm.eetool.close()
pre_jnts = [1.57, -1.66, -1.92, -1.12, 1.57, 0]
bot.arm.set_jpos(pre_jnts)
time.sleep(1)
ee_pose = bot.arm.get_ee_pose()
reset_pos = ee_pose[0]
print('Reset pos:', reset_pos)
while not rospy.is_shutdown():
try:
push(bot, reset_pos, z_lowest=args.z_min)
except Exception as e:
print(e)
time.sleep(1)
def main():
"""
Visualize the point cloud from the RGBD camera.
"""
robot = Robot('ur5e_2f140', pb=False, use_cam=True, use_arm=False)
cam_pos, cam_ori = read_cam_ext('ur5e')
robot.cam.set_cam_ext(cam_pos, cam_ori)
vis = open3d.visualization.Visualizer()
vis.create_window("Point Cloud")
pcd = open3d.geometry.PointCloud()
pts, colors = robot.cam.get_pcd(in_world=True, filter_depth=False)
pcd.points = open3d.utility.Vector3dVector(pts)
pcd.colors = open3d.utility.Vector3dVector(colors / 255.0)
vis.add_geometry(pcd)
while True:
pts, colors = robot.cam.get_pcd(in_world=True, filter_depth=False)
pcd.points = open3d.utility.Vector3dVector(pts)
pcd.colors = open3d.utility.Vector3dVector(colors / 255.0)
vis.update_geometry()
vis.poll_events()
vis.update_renderer()
time.sleep(0.1)
def main():
"""
This function demonstrates how to get joint information
such as joint positions/velocities/torques.
"""
robot = Robot('ur5e_2f140', pb_cfg={'gui': True})
robot.arm.go_home()
log_info('\nJoint positions for all actuated joints:')
jpos = robot.arm.get_jpos()
log_info(np.round(jpos, decimals=3))
joint = 'shoulder_pan_joint'
log_info('Joint [%s] position: %.3f' %
(joint, robot.arm.get_jpos('shoulder_pan_joint')))
log_info('Joint velocities:')
jvel = robot.arm.get_jvel()
log_info(np.round(jvel, decimals=3))
log_info('Joint torques:')
jtorq = robot.arm.get_jtorq()
log_info(np.round(jtorq, decimals=3))
robot.arm.eetool.close()
log_info('Gripper position (close): %.3f' % robot.arm.eetool.get_pos())
robot.arm.eetool.open()
log_info('Gripper position (open): %.3f' % robot.arm.eetool.get_pos())
def main(args):
cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
rospy.init_node('replay')
with open(os.join(args.data_dir, args.data_file), 'rb') as data_f:
data = pickle.load(data_f)
yumi_ar = Robot('yumi',
pb=True,
arm_cfg={
'render': True,
'self_collision': False
})
if args.object:
box_id = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '.urdf', cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:])
def worker_yumi(child_conn):
while True:
# print("here!")
try:
if not child_conn.poll(0.0001):
continue
msg = child_conn.recv()
except (EOFError, KeyboardInterrupt):
break
if msg == "RESET":
yumi = Robot('yumi', pb=True, arm_cfg={'render': True, 'self_collision': False})
# client_id = p.connect(p.DIRECT)
print("\n\nfinished worker construction\n\n")
continue
if msg == "HOME":
yumi.arm.go_home()
continue
if msg == "END":
break
print("before sleep!")
time.sleep(0.01)
print("breaking")
child_conn.close()
def main(args):
cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
rospy.init_node('MacroActions')
data = {}
data['saved_data'] = []
data['metadata'] = {}
# parent1, child1 = Pipe()
# parent2, child2 = Pipe()
# work_queue = Queue()
# result_queue = Queue()
# p1 = Process(target=worker_yumi, args=(child1, work_queue, result_queue, cfg, args))
# p2 = Process(target=worker_yumi, args=(child2, work_queue, result_queue, cfg, args))
# p1.start()
# p2.start()
# parent1.send("RESET")
# parent2.send("RESET")
# print("started workers")
# time.sleep(15.0)
# embed()
# # setup yumi
yumi_ar = Robot('yumi',
pb=True,
arm_cfg={
'render': args.visualize,
'self_collision': False,
'rt_simulation': False
})
yumi_ar.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT)
gel_id = 12
alpha = 0.01
K = 500
restitution = 0.99
dynamics_info = {}
dynamics_info['contactDamping'] = alpha * K
dynamics_info['contactStiffness'] = K
dynamics_info['rollingFriction'] = args.rolling
dynamics_info['restitution'] = restitution
p.changeDynamics(yumi_ar.arm.robot_id,
gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha * K,
rollingFriction=args.rolling)
# setup yumi_gs
# yumi_gs = YumiGelslimPybulet(yumi_ar, cfg, exec_thread=args.execute_thread)
yumi_gs = YumiCamsGS(yumi_ar, cfg, exec_thread=args.execute_thread)
if args.object:
box_id = pb_util.load_urdf(
args.config_package_path + 'descriptions/urdf/' +
args.object_name + '.urdf', cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:])
# trans_box_id = pb_util.load_urdf(
# args.config_package_path +
# 'descriptions/urdf/'+args.object_name+'_trans.urdf',
# cfg.OBJECT_POSE_3[0:3],
# cfg.OBJECT_POSE_3[3:]
# )
# setup macro_planner
action_planner = ClosedLoopMacroActions(cfg,
yumi_gs,
box_id,
pb_util.PB_CLIENT,
args.config_package_path,
replan=args.replan)
manipulated_object = None
object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT)
object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL)
palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT)
palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT)
example_args = {}
example_args['object_pose1_world'] = object_pose1_world
example_args['object_pose2_world'] = object_pose2_world
example_args['palm_pose_l_object'] = palm_pose_l_object
example_args['palm_pose_r_object'] = palm_pose_r_object
example_args['object'] = manipulated_object
# example_args['N'] = 60 # 60
example_args['N'] = calc_n(object_pose1_world, object_pose2_world) # 60
print("N: " + str(example_args['N']))
example_args['init'] = True
example_args['table_face'] = 0
primitive_name = args.primitive
mesh_file = args.config_package_path + 'descriptions/meshes/objects/' + args.object_name + '_experiments.stl'
exp_single = SingleArmPrimitives(cfg, box_id, mesh_file)
exp_double = DualArmPrimitives(cfg, box_id, mesh_file)
# trans_box_lock = threading.RLock()
# goal_viz = GoalVisual(
# trans_box_lock,
# trans_box_id,
# action_planner.pb_client,
# cfg.OBJECT_POSE_3)
# visualize_goal_thread = threading.Thread(
# target=goal_viz.visualize_goal_state)
# visualize_goal_thread.daemon = True
# visualize_goal_thread.start()
data['metadata']['mesh_file'] = mesh_file
data['metadata']['cfg'] = cfg
data['metadata']['dynamics'] = dynamics_info
data['metadata']['cam_cfg'] = yumi_gs.cam_cfg
if args.debug:
init_id = exp.get_rand_init(ind=2)[-1]
obj_pose_final = util.list2pose_stamped(exp.init_poses[init_id])
point, normal, face = exp.sample_contact(primitive_name)
# embed()
world_pose = exp.get_palm_pose_world_frame(
point, normal, primitive_name=primitive_name)
obj_pos_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[0])
obj_ori_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[1])
obj_pose_world = util.list2pose_stamped(obj_pos_world + obj_ori_world)
contact_obj_frame = util.convert_reference_frame(
world_pose, obj_pose_world, util.unit_pose())
example_args['palm_pose_r_object'] = contact_obj_frame
example_args['object_pose1_world'] = obj_pose_world
obj_pose_final = util.list2pose_stamped(exp.init_poses[init_id])
obj_pose_final.pose.position.z = obj_pose_world.pose.position.z / 1.175
print("init: ")
print(util.pose_stamped2list(object_pose1_world))
print("final: ")
print(util.pose_stamped2list(obj_pose_final))
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
plan = action_planner.get_primitive_plan(primitive_name, example_args,
'right')
embed()
import simulation
for i in range(10):
simulation.visualize_object(
object_pose1_world,
filepath=
"package://config/descriptions/meshes/objects/realsense_box_experiments.stl",
name="/object_initial",
color=(1., 0., 0., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
simulation.visualize_object(
object_pose2_world,
filepath=
"package://config/descriptions/meshes/objects/realsense_box_experiments.stl",
name="/object_final",
color=(0., 0., 1., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
rospy.sleep(.1)
simulation.simulate(plan)
else:
global_start = time.time()
for trial in range(20):
k = 0
while True:
# sample a random stable pose, and get the corresponding
# stable orientation index
k += 1
# init_id = exp.get_rand_init()[-1]
init_id = exp.get_rand_init(ind=0)[-1]
# sample a point on the object that is valid
# for the primitive action being executed
point, normal, face = exp.sample_contact(
primitive_name=primitive_name)
if point is not None:
break
if k >= 10:
print("FAILED")
return
# get the full 6D pose palm in world, at contact location
palm_pose_world = exp.get_palm_pose_world_frame(
point, normal, primitive_name=primitive_name)
# get the object pose in the world frame
# if trial == 0:
# parent1.send("OBJECT_POSE")
# elif trial == 1:
# parent2.send("OBJECT_POSE")
obj_pos_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[0])
obj_ori_world = list(
p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[1])
obj_pose_world = util.list2pose_stamped(obj_pos_world +
obj_ori_world)
# obj_pose_world = work_queue.get(block=True)
# transform the palm pose from the world frame to the object frame
contact_obj_frame = util.convert_reference_frame(
palm_pose_world, obj_pose_world, util.unit_pose())
# set up inputs to the primitive planner, based on task
# including sampled initial object pose and contacts,
# and final object pose
example_args['palm_pose_r_object'] = contact_obj_frame
example_args['object_pose1_world'] = obj_pose_world
obj_pose_final = util.list2pose_stamped(exp.init_poses[init_id])
obj_pose_final.pose.position.z /= 1.18
print("init: ")
print(util.pose_stamped2list(object_pose1_world))
print("final: ")
print(util.pose_stamped2list(obj_pose_final))
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
example_args['primitive_name'] = primitive_name
example_args['N'] = calc_n(obj_pose_world, obj_pose_final)
print("N: " + str(example_args['N']))
# if trial == 0:
# goal_viz.update_goal_state(exp.init_poses[init_id])
try:
# get observation (images/point cloud)
obs = yumi_gs.get_observation(obj_id=box_id)
# get start/goal (obj_pose_world, obj_pose_final)
start = util.pose_stamped2list(obj_pose_world)
goal = util.pose_stamped2list(obj_pose_final)
# get corners (from exp? that has mesh)
keypoints_start = np.array(exp.mesh_world.vertices.tolist())
keypoints_start_homog = np.hstack(
(keypoints_start, np.ones((keypoints_start.shape[0], 1))))
goal_start_frame = util.convert_reference_frame(
pose_source=obj_pose_final,
pose_frame_target=obj_pose_world,
pose_frame_source=util.unit_pose())
goal_start_frame_mat = util.matrix_from_pose(goal_start_frame)
keypoints_goal = np.matmul(goal_start_frame_mat,
keypoints_start_homog.T).T
# get contact (palm pose object frame)
contact_obj_frame = util.pose_stamped2list(contact_obj_frame)
contact_world_frame = util.pose_stamped2list(palm_pose_world)
contact_pos = open3d.utility.DoubleVector(
np.array(contact_world_frame[:3]))
kdtree = open3d.geometry.KDTreeFlann(obs['pcd_full'])
nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos,
1)[1][0]
nearest_pt_world = np.asarray(
obs['pcd_full'].points)[nearest_pt_ind]
# embed()
result = action_planner.execute(primitive_name, example_args)
sample = {}
sample['obs'] = obs
sample['start'] = start
sample['goal'] = goal
sample['keypoints_start'] = keypoints_start
sample['keypoints_goal'] = keypoints_goal
sample['transformation'] = util.pose_stamped2list(
goal_start_frame)
sample['contact_obj_frame'] = contact_obj_frame
sample['contact_world_frame'] = contact_world_frame
sample['contact_pcd'] = nearest_pt_world
sample['result'] = result
sample['planner_args'] = example_args
data['saved_data'].append(sample)
# if trial == 0:
# parent1.send("SAMPLE")
# elif trial == 1:
# parent2.send("SAMPLE")
# result = work_queue.get(block=True)
# if trial == 0:
# parent1.send("SAMPLE")
# elif trial == 1:
# parent2.send("SAMPLE")
# parent1.send("SAMPLE")
# parent2.send("SAMPLE")
# start = time.time()
# done = False
# result_list = []
# while (time.time() - start) < cfg.TIMEOUT and not done:
# try:
# result = result_queue.get(block=True)
# result_list.append(result)
# if len(result_list) == 2:
# done = True
# except result_queue.Empty:
# continue
# time.sleep(0.001)
print("reached final: " + str(result[0]))
except ValueError:
print("moveit failed!")
# time.sleep(0.1)
# yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
j_pos = cfg.RIGHT_INIT + cfg.LEFT_INIT
for ind, jnt_id in enumerate(yumi_ar.arm.arm_jnt_ids):
p.resetJointState(yumi_ar.arm.robot_id,
jnt_id,
targetValue=j_pos[ind])
# p.resetJointStatesMultiDof(
# yumi_ar.arm.robot_id,
# yumi_ar.arm.arm_jnt_ids,
# targetValues=j_pos)
# parent1.send("HOME")
# parent2.send("HOME")
# time.sleep(1.0)
# embed()
# embed()
print("TOTAL TIME: " + str(time.time() - global_start))
with open('data/sample_data_right_hand_pull.pkl', 'wb') as data_f:
pickle.dump(data, data_f)
def main(args):
cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml"
cfg = get_cfg_defaults()
cfg.merge_from_file(cfg_file)
cfg.freeze()
rospy.init_node('MacroActions')
signal.signal(signal.SIGINT, signal_handler)
data = {}
data['saved_data'] = []
data['metadata'] = {}
# parent1, child1 = Pipe()
# parent2, child2 = Pipe()
# work_queue = Queue()
# result_queue = Queue()
# p1 = Process(target=worker_yumi, args=(child1, work_queue, result_queue, cfg, args))
# p2 = Process(target=worker_yumi, args=(child2, work_queue, result_queue, cfg, args))
# p1.start()
# p2.start()
# parent1.send("RESET")
# parent2.send("RESET")
# print("started workers")
# time.sleep(15.0)
# embed()
# # setup yumi
# data_seed = 1
data_seed = args.np_seed
np.random.seed(data_seed)
yumi_ar = Robot('yumi_palms',
pb=True,
pb_cfg={'gui': args.visualize},
arm_cfg={'self_collision': False,
'seed': data_seed})
r_gel_id = cfg.RIGHT_GEL_ID
l_gel_id = cfg.LEFT_GEL_ID
alpha = cfg.ALPHA
K = cfg.GEL_CONTACT_STIFFNESS
restitution = cfg.GEL_RESTITUION
p.changeDynamics(
yumi_ar.arm.robot_id,
r_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha*K,
rollingFriction=args.rolling
)
p.changeDynamics(
yumi_ar.arm.robot_id,
l_gel_id,
restitution=restitution,
contactStiffness=K,
contactDamping=alpha*K,
rollingFriction=args.rolling
)
dynamics_info = {}
dynamics_info['contactDamping'] = alpha*K
dynamics_info['contactStiffness'] = K
dynamics_info['rollingFriction'] = args.rolling
dynamics_info['restitution'] = restitution
yumi_gs = YumiCamsGS(
yumi_ar,
cfg,
exec_thread=False,
sim_step_repeat=args.step_repeat)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
if args.object:
obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path +
'descriptions/urdf/'+args.object_name+'.urdf',
cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:]
)
goal_obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path +
'descriptions/urdf/'+args.object_name+'_trans.urdf',
cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:]
)
p.setCollisionFilterPair(yumi_ar.arm.robot_id, goal_obj_id, r_gel_id, -1, enableCollision=False)
p.setCollisionFilterPair(obj_id, goal_obj_id, -1, -1, enableCollision=False)
p.setCollisionFilterPair(yumi_ar.arm.robot_id, obj_id, r_gel_id, -1, enableCollision=True)
p.setCollisionFilterPair(yumi_ar.arm.robot_id, obj_id, 27, -1, enableCollision=True)
yumi_ar.pb_client.reset_body(
obj_id,
cfg.OBJECT_POSE_3[:3],
cfg.OBJECT_POSE_3[3:])
yumi_ar.pb_client.reset_body(
goal_obj_id,
cfg.OBJECT_POSE_3[:3],
cfg.OBJECT_POSE_3[3:])
manipulated_object = None
object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT)
object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL)
palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT)
palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT)
example_args = {}
example_args['object_pose1_world'] = object_pose1_world
example_args['object_pose2_world'] = object_pose2_world
example_args['palm_pose_l_object'] = palm_pose_l_object
example_args['palm_pose_r_object'] = palm_pose_r_object
example_args['object'] = manipulated_object
example_args['N'] = 60
example_args['init'] = True
example_args['table_face'] = 0
primitive_name = args.primitive
mesh_file = args.config_package_path + 'descriptions/meshes/objects/' + args.object_name + '_experiments.stl'
exp_single = SingleArmPrimitives(
cfg,
yumi_ar.pb_client.get_client_id(),
obj_id,
mesh_file)
if primitive_name == 'grasp' or primitive_name == 'pivot':
exp_double = DualArmPrimitives(
cfg,
yumi_ar.pb_client.get_client_id(),
obj_id,
mesh_file)
exp_running = exp_double
else:
exp_running = exp_single
# setup macro_planner
action_planner = ClosedLoopMacroActions(
cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
replan=args.replan,
object_mesh_file=mesh_file
)
data['metadata']['mesh_file'] = mesh_file
data['metadata']['cfg'] = cfg
data['metadata']['dynamics'] = dynamics_info
data['metadata']['cam_cfg'] = yumi_gs.cam_setup_cfg
data['metadata']['step_repeat'] = args.step_repeat
delta_z_height = 0.95
with open(args.config_package_path+'descriptions/urdf/'+args.object_name+'.urdf', 'rb') as f:
urdf_txt = f.read()
data['metadata']['object_urdf'] = urdf_txt
data['metadata']['delta_z_height'] = delta_z_height
data['metadata']['step_repeat'] = args.step_repeat
data['metadata']['seed'] = data_seed
metadata = data['metadata']
if args.multi:
cuboid_sampler = CuboidSampler(
'/root/catkin_ws/src/primitives/objects/cuboids/nominal_cuboid.stl',
pb_client=yumi_ar.pb_client)
cuboid_fname_template = '/root/catkin_ws/src/primitives/objects/cuboids/'
cuboid_manager = MultiBlockManager(
cuboid_fname_template,
cuboid_sampler,
robot_id=yumi_ar.arm.robot_id,
table_id=27,
r_gel_id=r_gel_id,
l_gel_id=l_gel_id)
yumi_ar.pb_client.remove_body(obj_id)
yumi_ar.pb_client.remove_body(goal_obj_id)
cuboid_fname = cuboid_manager.get_cuboid_fname()
obj_id, sphere_ids, mesh, goal_obj_id = \
cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=True,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
action_planner.update_object(obj_id, mesh_file)
trans_box_lock = threading.RLock()
goal_viz = GoalVisual(
trans_box_lock,
goal_obj_id,
action_planner.pb_client,
cfg.OBJECT_POSE_3)
pickle_path = os.path.join(
args.data_dir,
primitive_name,
args.experiment_name
)
if not os.path.exists(pickle_path):
os.makedirs(pickle_path)
data_manager = DataManager(pickle_path)
if args.save_data:
with open(os.path.join(pickle_path, 'metadata.pkl'), 'wb') as mdata_f:
pickle.dump(metadata, mdata_f)
if args.debug:
if args.multi:
cuboid_sampler.delete_cuboid(obj_id, goal_obj_id, sphere_ids)
cuboid_fname = cuboid_manager.get_cuboid_fname()
obj_id, sphere_ids, mesh, goal_obj_id = cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=True,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
goal_viz.update_goal_obj(goal_obj_id)
p.changeDynamics(
obj_id,
-1,
lateralFriction=0.4
)
action_planner.update_object(obj_id, mesh_file)
exp_running.initialize_object(obj_id, cuboid_fname)
print('Reset multi block!')
else:
cuboid_fname = '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids/realsense_box_experiments.stl'
for _ in range(args.num_obj_samples):
if primitive_name == 'pull':
init_id = exp_running.get_rand_init(ind=2)[-1]
obj_pose_final = util.list2pose_stamped(exp_running.init_poses[init_id])
point, normal, face = exp_running.sample_contact(primitive_name)
world_pose = exp_running.get_palm_poses_world_frame(
point,
normal,
primitive_name=primitive_name)
obj_pos_world = list(p.getBasePositionAndOrientation(
obj_id, yumi_ar.pb_client.get_client_id())[0])
obj_ori_world = list(p.getBasePositionAndOrientation(
obj_id, yumi_ar.pb_client.get_client_id())[1])
obj_pose_world = util.list2pose_stamped(obj_pos_world + obj_ori_world)
contact_obj_frame = util.convert_reference_frame(
world_pose, obj_pose_world, util.unit_pose())
example_args['palm_pose_r_object'] = contact_obj_frame
example_args['object_pose1_world'] = obj_pose_world
obj_pose_final = util.list2pose_stamped(exp_running.init_poses[init_id])
obj_pose_final.pose.position.z = obj_pose_world.pose.position.z/1.175
print("init: ")
print(util.pose_stamped2list(object_pose1_world))
print("final: ")
print(util.pose_stamped2list(obj_pose_final))
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
elif primitive_name == 'grasp':
k = 0
have_contact = False
contact_face = None
while True:
x, y, dq, q, init_id = exp_running.get_rand_init()
obj_pose_world_nom = exp_running.get_obj_pose()[0]
palm_poses_world = exp_running.get_palm_poses_world_frame(
init_id,
obj_pose_world_nom,
[x, y, dq])
# get_palm_poses_world_frame may adjust the
# initial object pose, so need to check it again
obj_pose_world = exp_running.get_obj_pose()[0]
if palm_poses_world is not None:
have_contact = True
break
k += 1
if k >= 10:
print("FAILED")
break
if have_contact:
obj_pose_final = exp_running.goal_pose_world_frame_mod
palm_poses_obj_frame = {}
for key in palm_poses_world.keys():
palm_poses_obj_frame[key] = util.convert_reference_frame(
palm_poses_world[key], obj_pose_world, util.unit_pose())
example_args['palm_pose_r_object'] = palm_poses_obj_frame['right']
example_args['palm_pose_l_object'] = palm_poses_obj_frame['left']
example_args['object_pose1_world'] = obj_pose_world
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
plan = action_planner.get_primitive_plan(primitive_name, example_args, 'right')
embed()
import simulation
for i in range(10):
simulation.visualize_object(
object_pose1_world,
filepath="package://config/descriptions/meshes/objects/cuboids/" + cuboid_fname.split('objects/cuboids')[1],
name="/object_initial",
color=(1., 0., 0., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
simulation.visualize_object(
object_pose2_world,
filepath="package://config/descriptions/meshes/objects/cuboids/" + cuboid_fname.split('objects/cuboids')[1],
name="/object_final",
color=(0., 0., 1., 1.),
frame_id="/yumi_body",
scale=(1., 1., 1.))
rospy.sleep(.1)
simulation.simulate(plan, cuboid_fname.split('objects/cuboids')[1])
else:
global_start = time.time()
face = 0
# exp_double.reset_graph(face)
start_time = time.time()
success = 0
for trial in range(args.num_trials):
k = 0
if args.multi:
cuboid_sampler.delete_cuboid(obj_id, goal_obj_id, sphere_ids)
cuboid_fname = cuboid_manager.get_cuboid_fname()
obj_id, sphere_ids, mesh, goal_obj_id = cuboid_sampler.sample_cuboid_pybullet(
cuboid_fname,
goal=True,
keypoints=False)
cuboid_manager.filter_collisions(obj_id, goal_obj_id)
goal_viz.update_goal_obj(goal_obj_id)
p.changeDynamics(
obj_id,
-1,
lateralFriction=0.4
)
action_planner.update_object(obj_id, mesh_file)
exp_running.initialize_object(obj_id, cuboid_fname)
print('Reset multi block!')
for _ in range(args.num_obj_samples):
while True:
have_contact = False
# sample a random stable pose, and get the corresponding
# stable orientation index
k += 1
if primitive_name == 'pull':
# init_id = exp_running.get_rand_init()[-1]
init_id = exp_running.get_rand_init()[-1]
# sample a point on the object that is valid
# for the primitive action being executed
point, normal, face = exp_running.sample_contact(
primitive_name=primitive_name)
if point is not None:
break
elif primitive_name == 'grasp':
x, y, dq, q, init_id = exp_double.get_rand_init()
obj_pose_world_nom = exp_double.get_obj_pose()[0]
palm_poses_world = exp_double.get_palm_poses_world_frame(
init_id,
obj_pose_world_nom,
[x, y, dq])
obj_pose_world = exp_double.get_obj_pose()[0]
if palm_poses_world is not None:
have_contact = True
break
if k >= 10:
print("FAILED")
return
# for _ in range(10):
# yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
if primitive_name == 'pull':
# get the full 6D pose palm in world, at contact location
palm_pose_world = exp_running.get_palm_poses_world_frame(
point,
normal,
primitive_name=primitive_name)
# get the object pose in the world frame
# if trial == 0:
# parent1.send("OBJECT_POSE")
# elif trial == 1:
# parent2.send("OBJECT_POSE")
obj_pos_world = list(p.getBasePositionAndOrientation(
obj_id,
yumi_ar.pb_client.get_client_id())[0])
obj_ori_world = list(p.getBasePositionAndOrientation(
obj_id,
yumi_ar.pb_client.get_client_id())[1])
obj_pose_world = util.list2pose_stamped(
obj_pos_world + obj_ori_world)
# obj_pose_world = work_queue.get(block=True)
# transform the palm pose from the world frame to the object frame
contact_obj_frame = util.convert_reference_frame(
palm_pose_world, obj_pose_world, util.unit_pose())
# set up inputs to the primitive planner, based on task
# including sampled initial object pose and contacts,
# and final object pose
example_args['palm_pose_r_object'] = contact_obj_frame
example_args['object_pose1_world'] = obj_pose_world
# obj_pose_final = util.list2pose_stamped(exp_running.init_poses[init_id])
x, y, q, _ = exp_running.get_rand_init(execute=False, ind=init_id)
final_nominal = exp_running.init_poses[init_id]
final_nominal[0] = x
final_nominal[1] = y
final_nominal[3:] = q
obj_pose_final = util.list2pose_stamped(final_nominal)
goal_viz.update_goal_state(final_nominal)
obj_pose_final.pose.position.z += cfg.TABLE_HEIGHT
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
example_args['primitive_name'] = primitive_name
example_args['N'] = exp_running.calc_n(
obj_pose_world, obj_pose_final)
elif primitive_name == 'grasp':
if have_contact:
obj_pose_final = exp_double.goal_pose_world_frame_mod
palm_poses_obj_frame = {}
for key in palm_poses_world.keys():
palm_poses_obj_frame[key] = util.convert_reference_frame(
palm_poses_world[key], obj_pose_world, util.unit_pose()
)
example_args['palm_pose_r_object'] = palm_poses_obj_frame['right']
example_args['palm_pose_l_object'] = palm_poses_obj_frame['left']
example_args['object_pose1_world'] = obj_pose_world
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
else:
continue
try:
obs, pcd = yumi_gs.get_observation(obj_id=obj_id)
start = util.pose_stamped2list(obj_pose_world)
goal = util.pose_stamped2list(obj_pose_final)
keypoints_start = np.array(exp_running.mesh_world.vertices.tolist())
keypoints_start_homog = np.hstack(
(keypoints_start, np.ones((keypoints_start.shape[0], 1)))
)
start_mat = util.matrix_from_pose(obj_pose_world)
goal_mat = util.matrix_from_pose(obj_pose_final)
T_mat = np.matmul(goal_mat, np.linalg.inv(start_mat))
keypoints_goal = np.matmul(T_mat, keypoints_start_homog.T).T[:, :3]
contact_obj_frame_dict = {}
contact_world_frame_dict = {}
nearest_pt_world_dict = {}
if primitive_name == 'pull':
active_arm, inactive_arm = action_planner.get_active_arm(
util.pose_stamped2list(obj_pose_world)
)
# get contact (palm pose object frame)
contact_obj_frame_dict[active_arm] = util.pose_stamped2list(contact_obj_frame)
contact_world_frame_dict[active_arm] = util.pose_stamped2list(palm_pose_world)
contact_pos = open3d.utility.DoubleVector(np.array(contact_world_frame_dict[active_arm][:3]))
kdtree = open3d.geometry.KDTreeFlann(pcd)
# nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0]
# nearest_pt_world_dict[active_arm] = np.asarray(pcd.points)[nearest_pt_ind]
contact_obj_frame_dict[inactive_arm] = None
contact_world_frame_dict[inactive_arm] = None
nearest_pt_world_dict[inactive_arm] = None
elif primitive_name == 'grasp':
for key in palm_poses_obj_frame.keys():
contact_obj_frame_dict[key] = util.pose_stamped2list(palm_poses_obj_frame[key])
contact_world_frame_dict[key] = util.pose_stamped2list(palm_poses_world[key])
contact_pos = open3d.utility.DoubleVector(np.array(contact_world_frame_dict[key][:3]))
kdtree = open3d.geometry.KDTreeFlann(pcd)
# nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0]
# nearest_pt_world_dict[key] = np.asarray(pcd.points)[nearest_pt_ind]
result = action_planner.execute(primitive_name, example_args)
if result is not None:
print('Trial number: ' + str(trial) + ', reached final: ' + str(result[0]))
print('Time so far: ' + str(time.time() - start_time))
if result[0]:
success += 1
sample = {}
sample['obs'] = obs
sample['start'] = start
sample['goal'] = goal
sample['keypoints_start'] = keypoints_start
sample['keypoints_goal'] = keypoints_goal
sample['transformation'] = util.pose_from_matrix(T_mat)
sample['contact_obj_frame'] = contact_obj_frame_dict
sample['contact_world_frame'] = contact_world_frame_dict
# sample['contact_pcd'] = nearest_pt_world_dict
sample['result'] = result
if primitive_name == 'grasp':
sample['goal_face'] = exp_double.goal_face
if args.save_data:
data_manager.save_observation(sample, str(trial))
print("Success: " + str(success))
else:
continue
# data['saved_data'].append(sample)
# if trial == 0:
# parent1.send("SAMPLE")
# elif trial == 1:
# parent2.send("SAMPLE")
# result = work_queue.get(block=True)
# if trial == 0:
# parent1.send("SAMPLE")
# elif trial == 1:
# parent2.send("SAMPLE")
# parent1.send("SAMPLE")
# parent2.send("SAMPLE")
# start = time.time()
# done = False
# result_list = []
# while (time.time() - start) < cfg.TIMEOUT and not done:
# try:
# result = result_queue.get(block=True)
# result_list.append(result)
# if len(result_list) == 2:
# done = True
# except result_queue.Empty:
# continue
# time.sleep(0.001)
except ValueError as e:
print("Value error: ")
print(e)
# time.sleep(1.0)
# pose = util.pose_stamped2list(yumi_gs.compute_fk(yumi_gs.get_jpos(arm='right')))
# pos, ori = pose[:3], pose[3:]
# # pose = yumi_gs.get_ee_pose()
# # pos, ori = pose[0], pose[1]
# # pos[2] -= 0.0714
# pos[2] += 0.001
# r_jnts = yumi_gs.compute_ik(pos, ori, yumi_gs.get_jpos(arm='right'))
# l_jnts = yumi_gs.get_jpos(arm='left')
# if r_jnts is not None:
# for _ in range(10):
# pos[2] += 0.001
# r_jnts = yumi_gs.compute_ik(pos, ori, yumi_gs.get_jpos(arm='right'))
# l_jnts = yumi_gs.get_jpos(arm='left')
# if r_jnts is not None:
# yumi_gs.update_joints(list(r_jnts) + l_jnts)
# time.sleep(0.1)
time.sleep(0.1)
for _ in range(10):
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
# for _ in range(10):
# j_pos = cfg.RIGHT_INIT + cfg.LEFT_INIT
# for ind, jnt_id in enumerate(yumi_ar.arm.arm_jnt_ids):
# p.resetJointState(
# yumi_ar.arm.robot_id,
# jnt_id,
# targetValue=j_pos[ind]
# )
# yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
# p.resetJointStatesMultiDof(
# yumi_ar.arm.robot_id,
# yumi_ar.arm.arm_jnt_ids,
# targetValues=j_pos)
# parent1.send("HOME")
# parent2.send("HOME")
# time.sleep(1.0)
print("TOTAL TIME: " + str(time.time() - global_start))
def worker_yumi(child_conn, work_queue, result_queue, cfg, args):
while True:
# print("here!")
try:
if not child_conn.poll(0.0001):
continue
msg = child_conn.recv()
except (EOFError, KeyboardInterrupt):
break
if msg == "RESET":
# yumi = Robot('yumi', pb=True, arm_cfg={'render': True, 'self_collision': False})
# client_id = p.connect(p.DIRECT)
# print("\n\nfinished worker construction\n\n")
yumi_ar = Robot('yumi',
pb=True,
arm_cfg={'render': True, 'self_collision': False})
yumi_ar.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT)
gel_id = 12
alpha = 0.01
K = 500
p.changeDynamics(
yumi_ar.arm.robot_id,
gel_id,
restitution=0.99,
contactStiffness=K,
contactDamping=alpha*K,
rollingFriction=args.rolling
)
# setup yumi_gs
yumi_gs = YumiGelslimPybulet(yumi_ar, cfg, exec_thread=args.execute_thread, sim_step_repeat=args.step_repeat)
obj_id = yumi_ar.pb_client.load_urdf(
args.config_package_path +
'descriptions/urdf/'+args.object_name+'.urdf',
cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:]
)
trans_box_id = yumi_ar.pb_client.load_urdf(
args.config_package_path +
'descriptions/urdf/'+args.object_name+'_trans.urdf',
cfg.OBJECT_POSE_3[0:3],
cfg.OBJECT_POSE_3[3:]
)
# setup macro_planner
action_planner = ClosedLoopMacroActions(
cfg,
yumi_gs,
obj_id,
yumi_ar.pb_client.get_client_id(),
args.config_package_path,
replan=args.replan
)
continue
if msg == "HOME":
yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT)
continue
if msg == "OBJECT_POSE":
obj_pos_world = list(p.getBasePositionAndOrientation(
obj_id,
yumi_ar.pb_client.get_client_id())[0])
obj_ori_world = list(p.getBasePositionAndOrientation(
obj_id,
yumi_ar.pb_client.get_client_id())[1])
obj_pose_world = util.list2pose_stamped(
obj_pos_world + obj_ori_world)
work_queue.put(obj_pose_world)
continue
if msg == "SAMPLE":
# try:
# example_args = work_queue.get(block=True)
# primitive_name = example_args['primitive_name']
# result = action_planner.execute(primitive_name, example_args)
# work_queue.put(result)
# except work_queue.Empty:
# continue
manipulated_object = None
object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT)
object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL)
palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT)
palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT)
example_args = {}
example_args['object_pose1_world'] = object_pose1_world
example_args['object_pose2_world'] = object_pose2_world
example_args['palm_pose_l_object'] = palm_pose_l_object
example_args['palm_pose_r_object'] = palm_pose_r_object
example_args['object'] = manipulated_object
example_args['N'] = 60 # 60
example_args['init'] = True
example_args['table_face'] = 0
primitive_name = args.primitive
mesh_file = args.config_package_path + 'descriptions/meshes/objects/' + args.object_name + '_experiments.stl'
exp_running = SingleArmPrimitives(cfg, obj_id, mesh_file)
k = 0
while True:
# sample a random stable pose, and get the corresponding
# stable orientation index
k += 1
# init_id = exp_running.get_rand_init()[-1]
init_id = exp_running.get_rand_init(ind=0)[-1]
# sample a point on the object that is valid
# for the primitive action being executed
point, normal, face = exp_running.sample_contact(
primitive_name=primitive_name)
if point is not None:
break
if k >= 10:
print("FAILED")
continue
# get the full 6D pose palm in world, at contact location
palm_pose_world = exp_running.get_palm_poses_world_frame(
point,
normal,
primitive_name=primitive_name)
obj_pos_world = list(p.getBasePositionAndOrientation(
obj_id,
yumi_ar.pb_client.get_client_id())[0])
obj_ori_world = list(p.getBasePositionAndOrientation(
obj_id,
yumi_ar.pb_client.get_client_id())[1])
obj_pose_world = util.list2pose_stamped(
obj_pos_world + obj_ori_world)
contact_obj_frame = util.convert_reference_frame(
palm_pose_world, obj_pose_world, util.unit_pose())
# set up inputs to the primitive planner, based on task
# including sampled initial object pose and contacts,
# and final object pose
example_args['palm_pose_r_object'] = contact_obj_frame
example_args['object_pose1_world'] = obj_pose_world
obj_pose_final = util.list2pose_stamped(exp_running.init_poses[init_id])
obj_pose_final.pose.position.z /= 1.155
print("init: ")
print(util.pose_stamped2list(object_pose1_world))
print("final: ")
print(util.pose_stamped2list(obj_pose_final))
example_args['object_pose2_world'] = obj_pose_final
example_args['table_face'] = init_id
example_args['primitive_name'] = primitive_name
# if trial == 0:
# goal_viz.update_goal_state(exp_running.init_poses[init_id])
result = None
try:
result = action_planner.execute(primitive_name, example_args)
# result = work_queue.get(block=True)
print("reached final: " + str(result[0]))
except ValueError:
print("moveit failed!")
result_queue.put(result)
continue
if msg == "END":
break
print("before sleep!")
time.sleep(0.01)
print("breaking")
child_conn.close()
def main():
"""
This function shows an example of block stacking.
"""
np.set_printoptions(precision=4, suppress=True)
robot = Robot('ur5e_2f140', pb=True, pb_cfg={'gui': False})
success = robot.arm.go_home()
if not success:
ar.log_warn('Robot go_home failed!!!')
#setup cam
robot_base = robot.arm.robot_base_pos
robot.cam.setup_camera(focus_pt=robot_base,
dist=3,
yaw=55,
pitch=-30,
roll=0)
out = cv2.VideoWriter('pickandplace.mp4', cv2.VideoWriter_fourcc(*'mp4v'),
30, (640, 480))
img = robot.cam.get_images(get_rgb=True, get_depth=False)[0]
out.write(np.array(img))
ori = euler2quat([0, 0, np.pi / 2])
robot.pb_client.load_urdf('table/table.urdf', [.5, 0, 0.4],
ori,
scaling=0.9)
box_size = 0.05
box_id1 = robot.pb_client.load_geom('box',
size=box_size,
mass=1,
base_pos=[.5, 0.12, 1.0],
rgba=[1, 0, 0, 1])
robot.arm.eetool.open()
record(robot, out)
obj_pos = robot.pb_client.get_body_state(box_id1)[0]
move_dir = obj_pos - robot.arm.get_ee_pose()[0]
move_dir[2] = 0
eef_step = 0.025
for i in range(3):
robot.arm.move_ee_xyz(move_dir / 3, eef_step=eef_step)
record(robot, out)
move_dir = np.zeros(3)
move_dir[2] = obj_pos[2] - robot.arm.get_ee_pose()[0][2]
for i in range(4):
robot.arm.move_ee_xyz(move_dir / 4, eef_step=eef_step)
record(robot, out)
robot.arm.eetool.close(wait=False)
record(robot, out)
for i in range(10):
robot.arm.move_ee_xyz([0, 0, 0.03], eef_step=eef_step)
record(robot, out)
for i in range(10):
robot.arm.move_ee_xyz([0.025, 0, 0.0], eef_step=eef_step)
record(robot, out)
for i in range(10):
robot.arm.move_ee_xyz([0, 0, -0.03], eef_step=eef_step)
record(robot, out)
robot.arm.eetool.open()
record(robot, out)
time.sleep(1)
record(robot, out)
for i in range(10):
robot.arm.move_ee_xyz([0, 0, 0.03], eef_step=eef_step)
record(robot, out)
time.sleep(3)
record(robot, out)
out.release()
