def test_ik():
sim = simulator.Simulator(view=True)
larm = arm.Arm('left', sim=sim)
larm.set_posture('mantis')
rarm = arm.Arm('right', sim=sim)
rarm.set_posture('mantis')
pose = rarm.get_pose()
position = pose.position
while True:
point = pose.position + np.array([
np.random.uniform(0, 1),
np.random.uniform(-.3, .3),
np.random.uniform(-.5, -.2)
])
sim.plot_point(sim.transform_from_to(point.array, 'base_link',
'world'),
color=(0, 1, 0),
size=.1)
joints = rarm.ik_lookat(position, point)
if joints is not None:
rarm.set_joints(joints)
else:
print('Joints is None')
sim.plot_transform(
sim.transform_from_to(np.array(rarm.get_pose().matrix),
'base_link', 'world'))
raw_input()
sim.clear_plots()
def extract_all():
larm = arm.Arm('left', view=False)
rarm = arm.Arm('right', view=False)
for f in os.listdir(data_folder):
f = data_folder + f
if f.endswith('.bag'):
print('Extracting info from: {0}'.format(f))
extract_info_from_bag(f, larm if 'push' in f else rarm)
def __init__(self):
self.pc = None
self.pc_sub = rospy.Subscriber('/cloud_pcd', sm.PointCloud2,
self._pc_callback)
self.sim = simulator.Simulator(view=True)
self.rarm = arm.Arm('right', sim=self.sim)
self.larm = arm.Arm('left', sim=self.sim)
self.cam = camera.Camera(self.arm, self.sim)
self.rarm.set_posture('mantis')
self.larm.set_posture('mantis')
def test_gripper_center_frame():
sim = simulator.Simulator(view=True)
larm = arm.Arm('left', sim=sim)
larm.set_posture('mantis')
rarm = arm.Arm('right', sim=sim)
rarm.set_posture('mantis')
print('Showing r_gripper_tool_frame')
sim.plot_transform(
sim.robot.GetLink('r_gripper_tool_frame').GetTransform())
raw_input()
print('Showing r_gripper_center_frame')
sim.plot_transform(
sim.robot.GetLink('r_gripper_center_frame').GetTransform())
raw_input()
def test_return_from_grasp_planner():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
p = planner.Planner('right', sim=sim)
rarm.set_posture('mantis')
curr_pose = rarm.get_pose()
curr_pose.orientation = tfx.tb_angles(0, 0, 180)
rarm.set_pose(curr_pose)
target_pose = rarm.get_pose() + [0.5, 0.4, -0.3]
target_pose.orientation *= tfx.tb_angles(25, -20, 10)
sim.plot_transform(target_pose.matrix)
start_joints = rarm.get_joints()
n_steps = 40
joint_traj = p.get_return_from_grasp_joint_trajectory(start_joints,
target_pose,
n_steps=n_steps)
poses = [tfx.pose(rarm.fk(joints)) for joints in joint_traj]
for pose in poses:
sim.plot_transform(pose.matrix)
print('Press enter to exit')
raw_input()
def test_grasp_planner():
sim = simulator.Simulator(view=True)
rarm = arm.Arm('right', sim=sim)
p = planner.Planner('right', sim=sim)
rarm.set_posture('mantis')
curr_pose = rarm.get_pose()
curr_pose.orientation = tfx.tb_angles(0, 0, 180)
rarm.set_pose(curr_pose)
target_pose = rarm.get_pose() + [0.5, 0.4, -0.3]
target_pose.orientation *= tfx.tb_angles(25, -20, 10)
sim.plot_transform(target_pose.matrix)
start_joints = rarm.get_joints()
n_steps = 20
joint_traj = p.get_grasp_joint_trajectory(
start_joints,
target_pose,
n_steps=n_steps,
ignore_orientation=True,
link_name='r_gripper_center_frame')
poses = [tfx.pose(rarm.fk(joints)) for joints in joint_traj]
for pose in poses:
sim.plot_transform(pose.matrix)
for n in xrange(n_steps - 1):
angle = (poses[n].orientation.inverse() * poses[-1].orientation).angle
print('angle[{0}]: {1}'.format(n, angle))
#IPython.embed()
print('Press enter to exit')
raw_input()
def test_kinect():
sim = simulator.Simulator(view=True)
larm = arm.Arm('left', sim=sim)
rarm = arm.Arm('right', sim=sim)
sim.add_box(rarm.get_pose(), [.5, .5, .5])
larm.set_posture('mantis')
rarm.set_posture('mantis')
manip_links = [
l for l in sim.robot.GetLinks()
if l not in rarm.manip.GetIndependentLinks()
]
while True:
print('Press "q" to exit')
if (utils.Getch.getch() == 'q'):
break
rarm.teleop()
is_coll = max([sim.env.CheckCollision(l) for l in manip_links])
print('Is in collision: {0}\n'.format(is_coll))
def __init__(self):
self.pc = None
self.pc_sub = rospy.Subscriber('/cloud_pcd', sm.PointCloud2,
self._pc_callback)
self.handle_pose = None
self.handle_pose_sub = rospy.Subscriber(
'/handle_detector/avg_handle_poses', gm.PoseArray,
self._avg_handle_poses_callback)
self.sim = simulator.Simulator(view=True)
self.arm = arm.Arm('right', sim=self.sim)
self.cam = camera.Camera(self.arm, self.sim)
self.planner = planner.Planner('right', sim=self.sim)
def __init__(self):
self.graspable_points = None
self.table_pose = None
self.table_extents = None
self.avg_handle_poses = None
self.is_reset_kinfu = False
self.home_pose = None
min_positions = np.array([0.25, -1, 0.3])
max_positions = np.array([1, 1, 1.5])
self.point_cloud_filter = lambda point: min(
min_positions < point) and min(point < max_positions)
# self.point_cloud_filter = lambda point: True
self.graspable_points_sub = rospy.Subscriber(
'/kinfu/graspable_points', sm.PointCloud2,
self._graspable_points_callback)
self.table_sub = rospy.Subscriber('/kinfu/plane_bounding_box',
pcl_utils.msg.BoundingBox,
self._table_callback)
self.avg_handle_poses_sub = rospy.Subscriber(
'/handle_detector/avg_handle_poses', gm.PoseArray,
self._avg_handle_poses_callback)
self.reset_kinfu_sub = rospy.Subscriber('/kinfu/reset',
std_msgs.msg.Empty,
self._reset_kinfu_callback)
self.home_pose_sub = rospy.Subscriber('/bsp/home_pose', gm.PoseStamped,
self._home_pose_callback)
self.grasp_joint_traj_pub = rospy.Publisher(
'/check_handle_grasp/grasp_joint_trajectory', tm.JointTrajectory)
self.return_grasp_joint_traj_pub = rospy.Publisher(
'/check_handle_grasp/return_grasp_joint_trajectory',
tm.JointTrajectory)
self.sim = simulator.Simulator(view=True)
self.arm = arm.Arm('right', sim=self.sim)
self.planner = planner.Planner('right', sim=self.sim)
self.arm.set_posture('mantis')
self.home_pose = self.arm.get_pose()
rospy.sleep(0.5)
self.sim.update()
rarm = arm.Arm('right', view=False)
for f in os.listdir(data_folder):
f = data_folder + f
if f.endswith('.bag'):
print('Extracting info from: {0}'.format(f))
extract_info_from_bag(f, larm if 'push' in f else rarm)
#########
# TESTS #
#########
if __name__ == '__main__':
rospy.init_node('analyze_data', anonymous=True)
parser = argparse.ArgumentParser()
parser.add_argument('bag_name')
args = parser.parse_args(rospy.myargv()[1:])
if args.bag_name == 'all':
extract_all()
else:
extract_info_from_bag(data_folder + args.bag_name,
arm.Arm('left', view=False))
print('Press enter to exit')
raw_input()
def test_gradient_sd():
sim = simulator.Simulator(view=True)
larm = arm.Arm('left', sim=sim)
larm.set_posture('mantis')
rarm = arm.Arm('right', sim=sim)
rarm.set_posture('mantis')
cam = camera.Camera(rarm, sim)
# triangles3d = []
# triangles3d = [geometry3d.Triangle([.7,0,.8],[.7,0,1.1],[.7,-.3,.7])]
# triangles3d = [geometry3d.Triangle([.5,0,.5],[.8,0,.6],[.5,-.3,.9])]
# triangles3d = [geometry3d.Triangle([np.random.uniform(.2,.5), np.random.uniform(-.5,0), np.random.uniform(.25,.75)],
# [np.random.uniform(.2,.5), np.random.uniform(-.5,0), np.random.uniform(.25,.75)],
# [np.random.uniform(.2,.5), np.random.uniform(-.5,0), np.random.uniform(.25,.75)]) for _ in xrange(3)]
table_center = np.array([.2, .7, .5])
triangles3d = [ #geometry3d.Triangle(table_center, table_center+np.array([.5,-1.4,0]), table_center+np.array([.5,0,0])),
#geometry3d.Triangle(table_center, table_center+np.array([0,-1.4,0]), table_center+np.array([.5,-1.4,0])),
geometry3d.Triangle(table_center + np.array([.25, -.7, 0]),
table_center + np.array([.25, -.7, .2]),
table_center + np.array([.25, -1.2, 0])),
geometry3d.Triangle(table_center + np.array([.25, -1.2, 0]),
table_center + np.array([.25, -.7, .2]),
table_center + np.array([.25, -1.2, .2]))
]
# point = np.array([0.5, 0.7, 0.66]) # sd: 0.115138180967
# point = np.array([0.5, -2, 0.66])
point = np.array([0.640, -0.170, 0.560]) # sd: 0.0189611173538
j = rarm.get_joints()
while True:
sim.clear_plots()
sim.plot_point(sim.transform_from_to(point, 'base_link', 'world'),
size=.02,
color=(0, 0, 1))
cam.plot(frame='base_link')
for tri3d in triangles3d:
tri3d.plot(sim, frame='base_link', fill=True)
sd = 0 #signed_distance(j, point, triangles3d, rarm, cam, plot=False)
start = time.time()
sd_grad = signed_distance_grad(j, point, triangles3d, rarm, cam)
elapsed = time.time() - start
start = time.time()
sd_grad_fast = fast_signed_distance_grad(j, point, triangles3d, rarm,
cam)
fast_elapsed = time.time() - start
print('sd: {0}'.format(sd))
print('sd_grad: {0}'.format(sd_grad))
print('sd_grad_fast: {0}'.format(sd_grad_fast))
print('sd_grad diff: {0}'.format(np.linalg.norm(sd_grad -
sd_grad_fast)))
print('sd_grad time: {0}'.format(elapsed))
print('sd_grad_fast time: {0}'.format(fast_elapsed))
print('Press enter to continue (or "q" to exit)')
char = utils.Getch.getch()
if char == 'q':
break
j -= (np.pi / 64.) * (sd_grad / np.linalg.norm(sd_grad))
rarm.set_joints(j)
