def reset(self, force_reset=False):
"""
Reset the simulation environment.
"""
if self._first_reset or force_reset:
self._pb.resetSimulation()
yumi_pos = self.cfgs.ARM.PYBULLET_RESET_POS
yumi_ori = arutil.euler2quat(self.cfgs.ARM.PYBULLET_RESET_ORI)
if self._self_collision:
colli_flag = {'flags': self._pb.URDF_USE_SELF_COLLISION}
self.robot_id = self._pb.loadURDF(self.cfgs.PYBULLET_URDF,
yumi_pos, yumi_ori,
**colli_flag)
else:
self.robot_id = self._pb.loadURDF(self.cfgs.PYBULLET_URDF,
yumi_pos, yumi_ori)
self._build_jnt_id()
self.setup_single_arms(right_arm=self.right_arm,
left_arm=self.left_arm)
for arm in self.arms.values():
if hasattr(arm, 'eetool'):
arm.eetool.feed_robot_info(self.robot_id, self.jnt_to_id)
arm.eetool.activate()
if arm._self_collision:
arm.eetool.disable_gripper_self_collision()
else:
self.go_home(ignore_physics=True)
for arm in self.arms.values():
if hasattr(arm, 'eetool'):
arm.eetool.close(ignore_physics=True)
self._first_reset = False
def auto_control(args, obj_id=None):
"""
Automatically read grasp pose from grasp pose file, this function only for grasp single object,
and object type, position, orientation must be the same with data_x/info.txt
Besides, set the position that input into control_robot() as the center point of contact point uniformly
Args:
args: parser
obj_id: pybullet object ID, used to reset the arena
"""
# Set the position as the center point of contact point uniformly
if args.method == 'GPNet':
poses, scores = load_pose_GPNet(
os.path.join(args.data_path, args.pose_file))
elif args.method == '6dof-graspnet':
poses, scores = load_pose_6dofgraspnet(
os.path.join(args.data_path, args.pose_file))
elif args.method == 'graspnet_baseline':
poses, scores = load_pose_graspnet_baseline(
os.path.join(args.data_path, args.pose_file))
else:
raise NotImplementedError
for pose in poses:
control_robot(pose,
robot_category=args.robot_arm,
control_mode=args.control_mode,
move_up=True)
time.sleep(3)
robot.pb_client.reset_body(obj_id,
base_pos=args.object_pos,
base_quat=ut.euler2quat(args.object_ori))
time.sleep(0.8)
print('all done!!!!')
input()
def load_objects(robot):
ori = euler2quat([0, 0, np.pi / 2])
robot.pb_client.load_urdf('table/table.urdf', [0.46, 0, 0],
ori,
scaling=0.9)
# sphere_id = robot.pb_client.load_geom('sphere',
# size=0.05,
# mass=1,
# base_pos=[0.5, 0, 0.55],
# rgba=[0, 1, 0, 1])
box_1_id = robot.pb_client.load_geom('box',
size=0.03,
mass=1,
base_pos=[0.35, -0.23, 0.55],
rgba=[1, 0, 0, 1])
box_2_id = robot.pb_client.load_geom('box',
size=[0.03, 0.012, 0.015],
mass=1,
base_pos=[0.53, 0.17, 0.55],
rgba=[0, 0, 1, 1])
cylinder_id = robot.pb_client.load_geom('cylinder',
size=[0.03, 0.04],
mass=1,
base_pos=[0.49, -0.12, 0.55],
rgba=[0, 1, 1, 1])
duck_id = robot.pb_client.load_geom('mesh',
mass=1,
visualfile='duck.obj',
mesh_scale=0.06,
base_pos=[0.63, 0.3, 0.55],
rgba=[0.5, 0.2, 1, 1])
def reset(self, force_reset=False):
"""
Reset the simulation environment.
"""
if self._first_reset or force_reset:
self._pb.resetSimulation()
yumi_pos = self.cfgs.ARM.PYBULLET_RESET_POS
yumi_ori = arutil.euler2quat(self.cfgs.ARM.PYBULLET_RESET_ORI)
if self._self_collision:
colli_flag = {'flags': self._pb.URDF_USE_SELF_COLLISION}
self.robot_id = self._pb.loadURDF(self.cfgs.PYBULLET_URDF,
yumi_pos, yumi_ori,
**colli_flag)
else:
self.robot_id = self._pb.loadURDF(self.cfgs.PYBULLET_URDF,
yumi_pos, yumi_ori)
self._build_jnt_id()
self.setup_single_arms(right_arm=self.right_arm,
left_arm=self.left_arm)
else:
self.go_home(ignore_physics=True)
self._first_reset = False
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 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 reach_pose_goal(pos, ori, get_func, object_id, pos_tol=0.01, ori_tol=0.02):
"""
Check if end effector reached goal or not. Returns true
if both position and orientation goals have been reached
within specified tolerance
Args:
pos (list np.ndarray): goal position
ori (list or np.ndarray): goal orientation. It can be:
**quaternion** ([qx, qy, qz, qw], shape: :math:`[4]`)
**rotation matrix** (shape: :math:`[3, 3]`)
**euler angles** ([roll, pitch, yaw], shape: :math:`[3]`)
get_func (function): name of the function with which we can get
the current pose
pos_tol (float): tolerance of position error
ori_tol (float): tolerance of orientation error
Returns:
bool: If goal pose is reached or not
"""
if not isinstance(pos, np.ndarray):
goal_pos = np.array(pos)
else:
goal_pos = pos
if not isinstance(ori, np.ndarray):
goal_ori = np.array(ori)
else:
goal_ori = ori
if goal_ori.size == 3:
goal_ori = common.euler2quat(goal_ori)
elif goal_ori.shape == (3, 3):
goal_ori = common.rot2quat(goal_ori)
elif goal_ori.size != 4:
raise TypeError('Orientation must be in one '
'of the following forms:'
'rotation matrix, euler angles, or quaternion')
goal_ori = goal_ori.flatten()
goal_pos = goal_pos.flatten()
new_ee_pos = np.array(get_func(object_id)[0])
new_ee_quat = get_func(object_id)[1]
pos_diff = new_ee_pos.flatten() - goal_pos
pos_error = np.max(np.abs(pos_diff))
quat_diff = common.quat_multiply(common.quat_inverse(goal_ori),
new_ee_quat)
rot_similarity = np.abs(quat_diff[3])
if pos_error < pos_tol and \
rot_similarity > 1 - ori_tol:
return True, pos_error, 1 - rot_similarity
else:
print("pos error: " + str(pos_error))
print("ori_error: " + str(1 - rot_similarity))
return False, pos_error, 1 - rot_similarity
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():
"""
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 reset(self):
self.robot.arm.reset()
self.robot.arm.go_home(ignore_physics=True)
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.box_id = self.robot.pb_client.load_geom('box', size=0.05, mass=1,
base_pos=[0.5, 0.12, 1.0],
rgba=[1, 0, 0, 1])
self.ref_ee_ori = self.robot.arm.get_ee_pose()[1]
self.gripper_ori = 0
return self._get_obs()
def perturb_box(yumi, box_id, delta_pos, delta_ori_euler=None):
object_pos = list(yumi.arm.p.getBasePositionAndOrientation(box_id)[0])
object_ori = list(yumi.arm.p.getBasePositionAndOrientation(box_id)[1])
new_pos = np.array(object_pos) + np.array(delta_pos)
if delta_ori_euler is not None:
pass
delta_ori_quat = common.euler2quat(delta_ori_euler)
new_ori = common.quat_multiply(object_ori, delta_ori_quat)
else:
new_ori = object_ori
yumi.arm.p.resetBasePositionAndOrientation(box_id, new_pos.tolist(),
new_ori)
def load_single_object(robot, args):
"""
Warnings: you need to set object type and size
Args:
robot:
args: args.object_pos, args.object_ori
"""
obj_id = robot.pb_client.load_geom(
shape_type='box',
size=0.025,
# shape_type='cylinder', # object type
# size=[0.022, 0.1], # object size
mass=0.3,
base_pos=args.object_pos,
base_ori=euler2quat(args.object_ori),
rgba=[1, 0, 0, 1])
return obj_id
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 reset(self, force_reset=False):
"""
Reset the simulation environment.
"""
self._pb.configureDebugVisualizer(self._pb.COV_ENABLE_RENDERING, 0)
if self._first_reset or force_reset:
self._pb.resetSimulation()
self.floor_id = self._pb.load_geom('box',
size=[10, 10, 0.01],
mass=0,
base_pos=[0, 0, 0],
rgba=[0.7, 0.77, 0.7, 1],
specular=[1, 1, 1, 1])
self.robot_base_pos = self.cfgs.ARM.PYBULLET_RESET_POS
robot_base_ori = self.cfgs.ARM.PYBULLET_RESET_ORI
self.robot_base_ori = arutil.euler2quat(robot_base_ori).tolist()
if self._self_collision:
colli_flag = self._pb.URDF_USE_SELF_COLLISION
self.robot_id = self._pb.loadURDF(self.cfgs.PYBULLET_URDF,
self.robot_base_pos,
self.robot_base_ori,
useFixedBase=True,
flags=colli_flag)
else:
self.robot_id = self._pb.loadURDF(self.cfgs.PYBULLET_URDF,
self.robot_base_pos,
self.robot_base_ori,
useFixedBase=True)
self._build_jnt_id()
# self.set_visual_shape()
if hasattr(self, 'eetool'):
self.eetool.feed_robot_info(self.robot_id, self.jnt_to_id)
self.eetool.activate()
if self._self_collision:
# weird behavior occurs on the gripper
# when self-collision is enforced
self.eetool.disable_gripper_self_collision()
else:
self.go_home(ignore_physics=True)
if hasattr(self, 'eetool'):
self.eetool.close(ignore_physics=True)
self._pb.configureDebugVisualizer(self._pb.COV_ENABLE_RENDERING, 1)
self._first_reset = False
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()
def main(ifRender=False):
"""
this function loads initial oboject on a table,
"""
np.set_printoptions(precision=3, suppress=True)
# load robot
robot = Robot('ur5e', use_eetool=False, arm_cfg={'render': ifRender, 'self_collision': True})
robot.arm.go_home()
origin = robot.arm.get_ee_pose()
def go_home():
robot.arm.set_ee_pose(home, origin[1])
# robot.arm.eetool.close()
# init robot
go_home()
# load table
table_ori = euler2quat([0, 0, np.pi/2.0])
table_pos = [table_x, table_y, table_z]
table_id = load_urdf('table/table.urdf', table_pos, table_ori, scaling=table_scaling)
# load box
box_id = load_geom('box', size=box_size, mass=1, base_pos=box_pos, rgba=[1, 0, 0, 1])
# init camera
def get_img():
# screenshot camera images
focus_pt = [0.7, 0, 1.]
robot.cam.setup_camera(focus_pt=focus_pt, dist=0.5, yaw=90, pitch=-60, roll=0)
rgb, depth = robot.cam.get_images(get_rgb=True, get_depth=True)
# crop the rgb
img = rgb[40:360, 0:640]
# dep = depth[40:360, 0:640]
# low pass filter : Gaussian blur
# blurred_img = cv2.GaussianBlur(img.astype('float32'), (5, 5), 0)
small_img = cv2.resize(img.astype('float32'), dsize=(200, 100),
interpolation=cv2.INTER_CUBIC) # numpy array dtype numpy int64 by default
# small_dep = cv2.resize(dep.astype('float32'), dsize=(200, 100),
# interpolation=cv2.INTER_CUBIC) # numpy array dtype numpy int64 by default
return small_img, depth
# test run
def test():
time_to_sleep = 0.5
go_home()
pose = robot.arm.get_ee_pose()
robot.arm.set_ee_pose([pose[0][0], pose[0][1], min_height], origin[1])
time.sleep(time_to_sleep)
get_img()
# test boundary
robot.arm.set_ee_pose([pose[0][0], pose[0][1]+table_length/2.0, min_height], origin[1])
# robot.arm.eetool.close()
time.sleep(time_to_sleep)
get_img()
robot.arm.move_ee_xyz([0, -table_length, 0])
time.sleep(time_to_sleep)
get_img()
pose = robot.arm.get_ee_pose()
robot.arm.set_ee_pose([pose[0][0], pose[0][1], rest_height], origin[1])
time.sleep(time_to_sleep)
get_img()
robot.arm.move_ee_xyz([0, table_length, 0])
time.sleep(time_to_sleep)
get_img()
pose = robot.arm.get_ee_pose()
robot.arm.set_ee_pose([pose[0][0], pose[0][1], min_height], origin[1])
time.sleep(time_to_sleep)
get_img()
# test arc
for i in list([np.pi/3.0, np.pi*2/5.0, np.pi*3/7.0, np.pi/2.0,
np.pi*4/7.0, np.pi*3/5.0, np.pi*2/3.0]):
robot.arm.set_ee_pose([arm_span*np.sin(i),
arm_span*np.cos(i), rest_height], origin[1])
time.sleep(time_to_sleep)
get_img()
robot.arm.set_ee_pose([arm_span*np.sin(i),
arm_span*np.cos(i), min_height], origin[1])
time.sleep(time_to_sleep)
get_img()
robot.arm.set_ee_pose([arm_span*np.sin(i),
arm_span*np.cos(i), rest_height], origin[1])
time.sleep(time_to_sleep)
get_img()
def get_pixel(X, Y, Z=0.975):
"""return fractional pixels representations
Z values comes from running robot.cam.get_pix.3dpt
representing the table surface height"""
ext_mat = robot.cam.get_cam_ext()
int_mat = robot.cam.get_cam_int()
XYZ = np.array([X, Y, Z, 1])
xyz = np.linalg.inv(ext_mat).dot(XYZ)[:3]
pixel_xyz = int_mat.dot(xyz)
pixel_xyz = pixel_xyz / pixel_xyz[2]
pixel_col = pixel_xyz[0] / 3.2 # due to image cropping and scaling
pixel_row = (pixel_xyz[1] - 40) / 3.2
return pixel_row, pixel_col
# # log object
# pos, quat, lin_vel, ang_vel = get_body_state(box_id)
# ar.log_info('Box:')
# ar.log_info(' position: %s' % np.array2string(pos, precision=2))
# ar.log_info(' quaternion: %s' % np.array2string(quat, precision=2))
# ar.log_info(' linear vel: %s' % np.array2string(lin_vel, precision=2))
# ar.log_info(' angular vel: %s' % np.array2string(ang_vel, precision=2))
# plt.figure()
# plt.imshow(rgb)
# plt.figure()
# plt.imshow(depth * 25, cmap='gray', vmin=0, vmax=255)
# print('Maximum Depth (m): %f' % np.max(depth))
# print('Minimum Depth (m): %f' % np.min(depth))
# plt.show()
def poke(save_dir='data/image'):
if not os.path.exists(save_dir):
os.makedirs(save_dir)
save_depth_dir = save_dir + '_depth'
if not os.path.exists(save_depth_dir):
os.makedirs(save_depth_dir)
index = 0
curr_img, curr_dep = get_img()
while True:
obj_pos, obj_quat, lin_vel, _ = get_body_state(box_id)
obj_ang = quat2euler(obj_quat)[2] # -pi ~ pi
obj_x, obj_y, obj_z = obj_pos
# check if cube is on table
if obj_z < box_z / 2.0 or lin_vel[0] > 1e-3: # important that box is still
print(obj_z, lin_vel[0])
reset_body(box_id, box_pos)
continue
while True:
# choose random poke point on the object
poke_x = np.random.random()*box_size + obj_x - box_size/2.0
poke_y = np.random.random()*box_size + obj_y - box_size/2.0
# choose poke angle along the z axis
poke_ang = np.random.random() * np.pi * 2 - np.pi
# choose poke length
poke_len = np.random.random() * poke_len_std + poke_len_mean
# calc starting poke location and ending poke loaction
start_x = poke_x - poke_len * np.cos(poke_ang)
start_y = poke_y - poke_len * np.sin(poke_ang)
end_x = poke_x + poke_len * np.cos(poke_ang)
end_y = poke_y + poke_len * np.sin(poke_ang)
if end_x > workspace_min_x and end_x < workspace_max_x \
and end_y > workspace_min_y and end_y < workspace_max_y:
break
robot.arm.move_ee_xyz([start_x-home[0], start_y-home[1], 0], 0.015)
robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015)
js1, js2, js3, js4, js5, js6 = robot.arm.get_jpos()
robot.arm.move_ee_xyz([end_x-start_x, end_y-start_y, 0], 0.015)
je1, je2, je3, je4, je5, je6 = robot.arm.get_jpos()
# important that we use move_ee_xyz, as set_ee_pose can throw obj in motion
robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015)
# move arm away from camera view
go_home() # important to have one set_ee_pose every loop to reset accu errors
next_img, next_dep = get_img()
# calc poke and obj locations in image pixel space
start_r, start_c = get_pixel(start_x, start_y)
end_r, end_c = get_pixel(end_x, end_y)
obj_r, obj_c = get_pixel(obj_x, obj_y)
with open(save_dir + '.txt', 'a') as file:
file.write('%d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n' % \
(index, start_x, start_y, end_x, end_y, obj_x, obj_y,
obj_quat[0], obj_quat[1], obj_quat[2], obj_quat[3],
js1, js2, js3, js4, js5, js6, je1, je2, je3, je4, je5, je6,
start_r, start_c, end_r, end_c, obj_r, obj_c))
cv2.imwrite(save_dir + '/' + str(index) +'.png',
cv2.cvtColor(curr_img, cv2.COLOR_RGB2BGR))
# cv2.imwrite(save_dir + '_depth/' + str(index) +'.png', curr_dep)
np.save(save_dir + '_depth/' + str(index), curr_dep)
curr_img = next_img
curr_dep = next_dep
if index % 1000 == 0:
ar.log_info('number of pokes: %sk' % str(index/1000))
index += 1
def test_data(filename):
data = np.loadtxt(filename, unpack=True)
idx, stx, sty, edx, edy, obx, oby, qt1, qt2, qt3, qt4, js1, js2, js3, js4, js5, js6, je1, je2, je3, je4, je5, je6 = data
go_home()
reset_body(box_id, box_pos)
_, _ = get_img()
for i in range(5):
robot.arm.set_ee_pose([stx[i], sty[i], rest_height], origin[1])
robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015)
robot.arm.move_ee_xyz([edx[i]-stx[i], edy[i]-sty[i], 0], 0.015)
robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015)
_, _ = get_img()
start_jpos = robot.arm.compute_ik([stx[i], sty[i], min_height])
end_jpos = robot.arm.compute_ik([edx[i], edy[i], min_height])
print('start')
print(js1[i], js2[i], js3[i], js4[i], js5[i], js6[i])
print(start_jpos)
print('end')
print(je1[i], je2[i], je3[i], je4[i], je5[i], je6[i])
print(end_jpos)
time.sleep(1)
def eval_poke(tag, ground_truth, img_idx, poke):
# ground_truth is the entire matrix from 'image_xx.txt' file
# img_idx is image index, first column of ground_truth
# poke is the predicted 4 vector for world, 8 for pixel and joint
# second half of the 8 vector are world coordinates computed from gt pixel and joint values
# label index in the poke data array (29, 1)
# index 0 is the poke index corresponding to starting image
stx, sty, edx, edy = 1, 2, 3, 4 # ee pos of start and end poke
obx, oby, qt1, qt2, qt3, qt4 = 5, 6, 7, 8, 9, 10 # obj pose before poke
js1, js2, js3, js4, js5, js6 = 11, 12, 13, 14, 15, 16 # jpos before poke
je1, je2, je3, je4, je5, je6 = 17, 18, 19, 20, 21, 22 # jpos after poke
sr, stc, edr, edc, obr, obc = 23, 24, 25, 26, 27, 28 # row and col locations in image
gt = ground_truth[img_idx:img_idx+2]
tgt_posi = gt[1, 5:8]
tgt_posi[-1] = box_z
tgt_quat = gt[1, 7:11]
init_posi = gt[0, 5:8]
init_posi[-1] = box_z
init_quat = gt[0, 7:11]
gt_poke = gt[0, 1:5]
go_home()
reset_body(box_id, init_posi, init_quat)
box_id2 = load_geom('box', size=box_size, mass=1,
base_pos=tgt_posi, base_ori=tgt_quat, rgba=[0,0,1,0.5])
p.setCollisionFilterPair(box_id, box_id2, -1, -1, enableCollision=0)
# to check robot_id and link_id
# robot.arm.robot_id
# robot.arm.p.getNumJoints(robot_id)
# robot.arm.p.getJointInfo(robot_id, 0-max_lnik_id)
p.setCollisionFilterPair(box_id2, 1, -1, 11, enableCollision=0)
_, _ = get_img()
# time.sleep(1)
# reset_body(box_id, init_posi, init_quat)
# robot.arm.move_ee_xyz([gt_poke[0]-home[0], gt_poke[1]-home[1], 0], 0.015)
# robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015)
# robot.arm.move_ee_xyz([gt_poke[2]-gt_poke[0], gt_poke[3]-gt_poke[1], 0], 0.015)
# robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015)
# go_home()
# _, _ = get_img()
# reset_body(box_id, init_posi, init_quat)
# poke = ground_truth[img_idx, 1:5]
robot.arm.move_ee_xyz([poke[0]-home[0], poke[1]-home[1], 0], 0.015)
robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015)
robot.arm.move_ee_xyz([poke[2]-poke[0], poke[3]-poke[1], 0], 0.015)
robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015)
go_home()
_, _ = get_img()
curr_posi, _, _, _ = get_body_state(box_id)
dist = np.sqrt((tgt_posi[0]-curr_posi[0])**2 + (tgt_posi[1]-curr_posi[1])**2)
print(dist)
# time.sleep(0.5)
remove_body(box_id2)
return dist
def calc_dist(gtfile, pdfile, tag, test_num=50):
# this function generate 50 visualization sequence
true = np.loadtxt(gtfile)
pred = np.loadtxt(pdfile)
accu_dist = 0.0
# query = random.sample(range(0, pred.shape[0]), test_num)
total = pred.shape[0]
query = range(total)[::total/test_num]
for i in query:
img_idx = int(pred[i][0])
if tag == 'world':
poke = pred[i][1:5]
elif tag == 'pixel':
rows = np.array([pred[i, 1], pred[i, 3], pred[i, 7], pred[i, 9]])
cols = np.array([pred[i, 2], pred[i, 4], pred[i, 8], pred[i, 10]])
rows = ((rows*3.2 + 40) + 0.5).astype(int)
cols = ((cols*3.2) + 0.5).astype(int)
depth_file = gtfile[:-4] + '_depth/' + str(img_idx) + '.npy'
depth_im = np.load(depth_file)
pokes_3d = get_pix_3dpt(depth_im, rows, cols)
poke = pokes_3d[:, :2].flatten()
else:
raise Exception("experiment_tag has to be 'world', 'joint', or 'pixel'")
accu_dist += eval_poke(tag, true, img_idx, poke)
return accu_dist / len(query)
def get_pix_3dpt(depth_im, rs, cs, in_world=True, filter_depth=False,
k=1, ktype='median', depth_min=None, depth_max=None):
if not isinstance(rs, int) and not isinstance(rs, list) and \
not isinstance(rs, np.ndarray):
raise TypeError('rs should be an int, a list or a numpy array')
if not isinstance(cs, int) and not isinstance(cs, list) and \
not isinstance(cs, np.ndarray):
raise TypeError('cs should be an int, a list or a numpy array')
if isinstance(rs, int):
rs = [rs]
if isinstance(cs, int):
cs = [cs]
if isinstance(rs, np.ndarray):
rs = rs.flatten()
if isinstance(cs, np.ndarray):
cs = cs.flatten()
if not (isinstance(k, int) and (k % 2) == 1):
raise TypeError('k should be a positive odd integer.')
# _, depth_im = self.get_images(get_rgb=False, get_depth=True)
if k == 1:
depth_im = depth_im[rs, cs]
else:
depth_im_list = []
if ktype == 'min':
ktype_func = np.min
elif ktype == 'max':
ktype_func = np.max
elif ktype == 'median':
ktype_func = np.median
elif ktype == 'mean':
ktype_func = np.mean
else:
raise TypeError('Unsupported ktype:[%s]' % ktype)
for r, c in zip(rs, cs):
s = k // 2
rmin = max(0, r - s)
rmax = min(self.img_height, r + s + 1)
cmin = max(0, c - s)
cmax = min(self.img_width, c + s + 1)
depth_im_list.append(ktype_func(depth_im[rmin:rmax,
cmin:cmax]))
depth_im = np.array(depth_im_list)
depth = depth_im.reshape(-1) * 1 #self.depth_scale
img_pixs = np.stack((rs, cs)).reshape(2, -1)
img_pixs[[0, 1], :] = img_pixs[[1, 0], :]
depth_min = depth_min if depth_min else 0.01 #self.depth_min
depth_max = depth_max if depth_max else 10 #self.depth_max
if filter_depth:
valid = depth > depth_min
valid = np.logical_and(valid,
depth < depth_max)
depth = depth[:, valid]
img_pixs = img_pixs[:, valid]
get_img() # to set up camera matrices
cam_int_mat_inv = robot.cam.cam_int_mat_inv
cam_ext_mat = robot.cam.cam_ext_mat
uv_one = np.concatenate((img_pixs,
np.ones((1, img_pixs.shape[1]))))
uv_one_in_cam = np.dot(cam_int_mat_inv, uv_one)
pts_in_cam = np.multiply(uv_one_in_cam, depth)
if in_world:
if cam_ext_mat is None:
raise ValueError('Please call set_cam_ext() first to set up'
' the camera extrinsic matrix')
pts_in_cam = np.concatenate((pts_in_cam,
np.ones((1, pts_in_cam.shape[1]))),
axis=0)
pts_in_world = np.dot(cam_ext_mat, pts_in_cam)
pts_in_world = pts_in_world[:3, :].T
return pts_in_world
else:
return pts_in_cam.T
from IPython import embed
embed()
