def get_group_for_joint(joint_names, robot=None, smallest=False, groups=None): """Given list of joints find the most appropriate group. INPUT link_ids [List of Strings] robot [RobotCommander] smallest [Bool] OUTPUT group [String] """ if not robot: robot = RobotCommander() if not groups: groups = robot.get_group_names() group = None n = np.Inf if smallest else 0 for g in groups: names = robot.get_joint_names(g) if all_in(joint_names, names): if smallest and len(names) < n: n = len(names) group = g elif len(names) > n: n = len(names) group = g return group
def print_joint_bounds(robot=None): if not robot: robot = RobotCommander() print 'NAME'.center(16), 'MIN'.center(10), 'MAX'.center(10) for name in robot.get_joint_names(): joint = robot.get_joint(name) bounds = joint.bounds() if bounds: print '%15s % 10.3f % 10.3f' % (name, bounds[0], bounds[1])
class TestLimits(unittest.TestCase): def setUp(self): self.robot = RobotCommander() self.bounds = list() for name in self.robot.get_joint_names(): joint = self.robot.get_joint(name) bounds = joint.bounds() if bounds: self.bounds.append((name, bounds)) def test_min_max(self): """Test whether the min is actually lower than max.""" failed = list() for name, bounds in self.bounds: if bounds[0] > bounds[1]: failed.append(name) self.assertFalse(failed, "Bounds are not valid for: %s" % " ".join(failed))
class pick_place(): def __init__(self): moveit_commander.roscpp_initialize(sys.argv) self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.ur5 = MoveGroupCommander("manipulator") self.gripper = MoveGroupCommander("end_effector") print("======================================================") print(self.robot.get_group_names()) print(self.robot.get_link_names()) print(self.robot.get_joint_names()) print(self.robot.get_planning_frame()) print(self.ur5.get_end_effector_link()) print("======================================================") self.ur5.set_max_velocity_scaling_factor(0.2) self.ur5.set_max_acceleration_scaling_factor(0.2) self.ur5.set_end_effector_link("fts_toolside") self.ur5.set_planning_time(10.0) #self.ur5.set_planner_id("RRTkConfigDefault") self.gripper_ac = RobotiqActionClient('icl_phri_gripper/gripper_controller') self.gripper_ac.wait_for_server() self.gripper_ac.initiate() self.gripper_ac.send_goal(0.08) self.gripper_ac.wait_for_result() self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb) def single_exuete(self, position, mode): offset = 0.01 rospy.loginfo("let do a single exuete") rospy.sleep(1) position_copy = deepcopy(position) position_copy += [0.14] position_copy[1] = position_copy[1] + offset pre_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2]) post_position = define_grasp([position_copy[0], position_copy[1], position_copy[2] + 0.2]) grasp_position = define_grasp(position_copy) self.ur5.set_pose_target(pre_position) rospy.loginfo("let's go to pre position") self.ur5.go() self.ur5.stop() self.ur5.clear_pose_targets() rospy.sleep(1) self.ur5.set_pose_target(grasp_position) rospy.loginfo("let's do this") self.ur5.go() self.ur5.stop() self.ur5.clear_pose_targets() if mode == "pick": self.gripper_ac.send_goal(0) if mode == "place": self.gripper_ac.send_goal(0.08) self.gripper_ac.wait_for_result() rospy.loginfo("I got this") rospy.sleep(1) self.ur5.set_pose_target(post_position) rospy.loginfo("move out") self.ur5.go() self.ur5.stop() self.ur5.clear_pose_targets() rospy.sleep(1) def pair_exuete(self, pick_position, place_position): rospy.loginfo("here we go pair") if pick_position and place_position: self.single_exuete(pick_position, "pick") self.single_exuete(place_position, "place") #rospy.sleep(1) rospy.loginfo("let's go and get some rest") rest_position = define_grasp([0.486, -0.152, 0.342]) self.ur5.set_pose_target(rest_position) self.ur5.go() self.ur5.stop() self.ur5.clear_pose_targets() rospy.sleep(1) def pickplace_cb(self, msg): #print(msg) print(msg.data) a = list(msg.data) mean_x = np.mean([a[i] for i in range(0, len(a)-2, 2)]) mean_y = np.mean([a[i] for i in range(1, len(a)-2, 2)]) num_goals = (len(msg.data) -2)/2 rospy.loginfo("there is {} goals".format(num_goals)) for i in range(0, len(a)-2, 2): pick_x, pick_y = coord_converter(msg.data[i], msg.data[i+1]) leeway_x = int(msg.data[i] - mean_x) leeway_y = int(msg.data[i+1] - mean_y) place_x, place_y = coord_converter(msg.data[-2] + leeway_x, msg.data[-1] + leeway_y) print(pick_x, pick_y) print(place_x, place_y) self.pair_exuete([pick_x, pick_y], [place_x, place_y])