def test_clip_triangle(): sim = simulator.Simulator(view=True) view_frustum = ViewFrustum((.6,.1,0), (.8,.2,.5), (.4,.1,0), (.2,.2,.5), (.4,-.1,0), (.2,-.2,.5), (.6,-.1,0), (.8,-.2,.5)) view_frustum.plot(sim, color=(1,0,0), fill=True, alpha=.25) for halfspace in view_frustum.halfspaces: halfspace.plot(sim, color=(0,0,1)) # triangle = Triangle([.5, 0, .25], [.5, 0, .6], [.52, .02, .25]) # triangle = Triangle([.5, 0, .25], [.7, .5, .5], [.9, .1, .6]) triangle = Triangle([.6, .1, -.2], [.7, .2, .3], [.5, 0, .1]) triangle.plot(sim, color=(0,0,1)) print('Original triangle, press enter to clip') raw_input() # sim.clear_plots(3) clipped_triangles = view_frustum.clip_triangle(triangle) print('Number of clipped triangles: {0}'.format(len(clipped_triangles))) for tri in clipped_triangles: tri.plot(sim, color=(0,1,0)) print('Press enter to exit') raw_input()
def test_pyramid_sd(): sim = simulator.Simulator(view=True) trunc_pyramid = TruncatedPyramid((.8,.2,1), (.8,.2,1.5), (.2,.2,1), (.2,.2,1.5), (.2,-.2,1), (.2,-.2,1.5)) trunc_pyramid.plot(sim, frame='base_link', color=(0,1,0), fill=True, alpha=.25) for halfspace in trunc_pyramid.halfspaces: halfspace.plot(sim, frame='base_link', color=(0,0,1)) pos_step = .01 delta_position = {'a' : [0, pos_step, 0], 'd' : [0, -pos_step, 0], 'w' : [pos_step, 0, 0], 'x' : [-pos_step, 0, 0], '+' : [0, 0, pos_step], '-' : [0, 0, -pos_step]} point = np.array((.7,.2,1), dtype=float) print('Move point around with keyboard to test signed-distance') char = '' while char != 'q': char = utils.Getch.getch() sim.clear_plots(1) if delta_position.has_key(char): point += np.array(delta_position[char]) sim.plot_point(sim.transform_from_to(point, 'base_link', 'world'), size=.02, color=(0,0,1)) is_inside = trunc_pyramid.is_inside(point) sd = trunc_pyramid.signed_distance(point) print('is inside: {0}'.format(is_inside)) print('sd: {0}\n'.format(sd))
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_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 __init__(self, arm_name, sim=None, interact=False): """ :param arm_name: "left" or "right" :param sim: OpenRave simulator (or create if None) :param interact: enable trajopt viewer """ assert arm_name == 'left' or arm_name == 'right' self.sim = sim if self.sim is None: self.sim = simulator.Simulator() self.robot = self.sim.robot self.manip = self.sim.larm if arm_name == 'left' else self.sim.rarm wrist_flex_index = self.robot.GetJointIndex(arm_name[0] + '_wrist_flex_joint') lower, upper = self.robot.GetDOFLimits() lower[wrist_flex_index], upper[ wrist_flex_index] = -np.pi / 2., np.pi / 2. self.robot.SetDOFLimits(lower, upper) if interact: trajoptpy.SetInteractive(True) self.tool_frame = '{0}_gripper_tool_frame'.format(arm_name[0]) self.joint_indices = self.manip.GetArmIndices()
def test_box(): sim = simulator.Simulator(view=True) pose = tfx.pose([1,0,0], tfx.tb_angles(0,45,0)) extents = [0.1,0.1,0.1] sim.add_box(pose.matrix, extents) print('Press enter to exit') raw_input()
def test_mantis(): sim = simulator.Simulator(view=True) rarm = arm.Arm('right', sim=sim) larm = arm.Arm('left', sim=sim) rarm.go_to_posture('mantis', block=False) larm.go_to_posture('mantis', block=False) rarm.open_gripper() larm.open_gripper()
def test_gripper(): sim = simulator.Simulator(view=True) rarm = arm.Arm('right', sim=sim) larm = arm.Arm('left', sim=sim) # rarm.close_gripper() # larm.close_gripper() rarm.open_gripper() larm.open_gripper()
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_distance_to_plot(): sim = simulator.Simulator(view=True) t = Triangle(np.random.rand(3), np.random.rand(3), np.random.rand(3)) t.plot(sim) p = 2*np.random.rand(3) closest_pt = t.closest_point_to(p) sim.plot_point(p, color=(0,0,1)) sim.plot_point(closest_pt) IPython.embed()
def __init__(self, arm_name, sim=None, default_speed=.05): """ :param arm_name: "left" or "right" :param sim: OpenRave simulator (or create if None) :param default_speed: speed in meters/second """ assert arm_name == 'left' or arm_name == 'right' self.arm_name = arm_name self.default_speed = default_speed self.tool_frame = '{0}_gripper_tool_frame'.format(arm_name[0]) self.joint_names = [ '{0}{1}'.format(arm_name[0], joint_name) for joint_name in self.joint_name_suffixes ] self.gripper_joint_name = '{0}_gripper_joint'.format(arm_name[0]) self.min_grasp, self.max_grasp = -.01, .1 self.default_max_effort = 40 # Newtons self.current_joints, self.current_grasp = None, None self.joint_state_sub = rospy.Subscriber('/joint_states', sm.JointState, self._joint_state_callback) rospy.loginfo('Waiting for /joint_states...') while not rospy.is_shutdown( ) and self.current_joints is None or self.current_grasp is None: rospy.sleep(.01) self.joint_command_pub = rospy.Publisher( '{0}_arm_controller/command'.format(arm_name[0]), tm.JointTrajectory) self.joint_command_client = actionlib.SimpleActionClient( '/{0}_arm_controller/joint_trajectory_action'.format(arm_name[0]), pcm.JointTrajectoryAction) rospy.loginfo('Waiting for joint command server...') self.joint_command_client.wait_for_server() #self.gripper_command_pub = rospy.Publisher('{0}_gripper_controller/command'.format(arm_name[0]), pcm.Pr2GripperCommand) self.gripper_command_client = actionlib.SimpleActionClient( '{0}_gripper_controller/gripper_action'.format(arm_name[0]), pcm.Pr2GripperCommandAction) rospy.loginfo('Waiting for gripper command server...') self.gripper_command_client.wait_for_server() self.sim = sim if self.sim is None: self.sim = simulator.Simulator() self.manip = self.sim.larm if arm_name == 'left' else self.sim.rarm rospy.sleep(1)
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 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 __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 __init__(self): #self.grasp_traj = None #self.return_grasp_traj = None #self.grasp_traj_sub = rospy.Subscriber('/check_handle_grasps/grasp_trajectory', gm.PoseArray, self._grasp_traj_callback) #self.return_grasp_traj_sub = rospy.Subscriber('/check_handle_grasps/return_grasp_trajectory', gm.PoseArray, self._return_grasp_traj_callback) self.grasp_joint_traj = None self.return_grasp_joint_traj = None self.grasp_joint_traj_sub = rospy.Subscriber( '/check_handle_grasp/grasp_joint_trajectory', tm.JointTrajectory, self._grasp_joint_traj_callback) self.return_grasp_joint_traj_sub = rospy.Subscriber( '/check_handle_grasp/return_grasp_joint_trajectory', tm.JointTrajectory, self._return_grasp_joint_traj_callback) self.sim = simulator.Simulator(view=True) self.arm = arm.Arm('right', sim=self.sim) self.speed = 0.06
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 test_plotting(): sim = simulator.Simulator(view=True) sim.plot_triangle(([1,0,1],[1,0,2],[1.5,0,1.5]),color=(1,0,0),alpha=0.5) view_frustum = ViewFrustum((.6,.1,0), (.8,.2,.5), (.4,.1,0), (.2,.2,.5), (.4,-.1,0), (.2,-.2,.5), (.6,-.1,0), (.8,-.2,.5)) view_frustum.plot(sim, color=(1,0,0), fill=True, alpha=.25) for halfspace in view_frustum.halfspaces: halfspace.plot(sim, color=(0,0,1)) trunc_pyramid = TruncatedPyramid((.6,.1,1), (.8,.2,1.5), (.4,.1,1), (.2,.2,1.5), (.4,-.1,1), (.2,-.2,1.5)) trunc_pyramid.plot(sim, color=(0,1,0), fill=True, alpha=.25) for halfspace in trunc_pyramid.halfspaces: halfspace.plot(sim, color=(0,0,1)) print('Press enter to exit') raw_input()
def test_joints(): sim = simulator.Simulator(view=True) rarm = arm.Arm('right', sim=sim) larm = arm.Arm('left', sim=sim) print('rarm joints: {0}'.format(rarm.get_joints()))
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)