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 __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()
def test_planner():
arm_name = 'right'
sim = simulator.Simulator(view=False)
a = arm.Arm(arm_name, sim=sim)
p = planner.Planner(arm_name, sim=sim, interact=False)
a.go_to_posture('untucked', speed=.2)
current_joints = a.get_joints()
current_pose = a.get_pose()
target_pose = current_pose + [.2, 0, 0]
print('Calling trajopt')
joint_traj = p.get_joint_trajectory(current_joints, target_pose)
print('\ndesired end pose:\n{0}'.format(target_pose.matrix))
print('joint_traj end pose:\n{0}'.format(a.fk(joint_traj[-1]).matrix))
IPython.embed()
def __init__(self):
self.arm_name = 'right'
self.sim = simulator.Simulator(view=True)
self.arm = arm.Arm(self.arm_name, sim=self.sim)
self.planner = planner.Planner(self.arm_name,
sim=self.sim,
interact=False)
self.sim.env.Load('../envs/SDHtable.xml')
#self.arm.go_to_posture('mantis')
self.home_pose = self.arm.get_pose()
self.handle_pose_sub = rospy.Subscriber('/localization/handle_pose',
geom_msg.PoseStamped,
self._handle_pose_callback)
self.handle_pose_callback, self.handle_pose = None, None
self.handle_pose_pub = rospy.Publisher('/debug_handle_pose',
geom_msg.PoseStamped)
rospy.loginfo('GreedyGrasp initialized')