class MoveitPlanner: def __init__(self, manipulator_side = 'left'): # Enable the robot at start and disable at the end BaxterRobot().EnableBaxterRobot() rospy.on_shutdown(BaxterRobot().DisableBaxterRobot) # Test Open Both Grippers and Close Them # baxter_manipulator = BaxterManipulatorAndGripper() # baxter_manipulator.SetupBaxterManipualtor() # baxter_manipulator.MoveBaxterManipulatorToNeutralPose() # baxter_manipulator.SetupBaxterGrippers() # baxter_manipulator.OpenGripper('left') # baxter_manipulator.OpenGripper('right') # rospy.sleep(3.0) # baxter_manipulator.CloseGripper('left') # baxter_manipulator.CloseGripper('right') # Referenceing baxter arm self.__manipulator_name = manipulator_side self.limb = MoveGroupCommander("{}_arm".format(manipulator_side)) self.limb.set_end_effector_link("{}_gripper".format(manipulator_side)) # Baxter Model configuration with MoveIt not configured to control its gripper, # thus using Baxter SDK API gripper class baxter_manipulator = BaxterManipulatorAndGripper() baxter_manipulator.SetupBaxterChosenGripperAndManipulator(manipulator_side) self.limb.set_planner_id("RRTConnectKConfigDefualt") self.limb.set_goal_position_tolerance(0.01) self.limb.set_goal_orientation_tolerance(0.01)
class MoveItArm: """ Represents Baxter's Arm in MoveIt! world. Represents a hand of Baxter through MoveIt! Group. Some reusable code has been packed together into this class to make the code more readable. """ def __init__(self, side_name): """Will configure and initialise Baxter's hand.""" self._side_name = side_name # This is the reference to Baxter's arm. self.limb = MoveGroupCommander("{}_arm".format(side_name)) self.limb.set_end_effector_link("{}_gripper".format(side_name)) # Unfortunetly, MoveIt was not able to make the gripper work, neither # did was able to find a very good pose using Forward Kinematics, # so instead the Baxter SDK is used here to do these two jobs. self.gripper = Gripper(side_name, CHECK_VERSION) self.gripper.calibrate() self._limb = Limb(side_name) # This solver seems to be better for finding solution among obstacles self.limb.set_planner_id("RRTConnectkConfigDefault") # Error tollerance should be as low as possible for better accuracy. self.limb.set_goal_position_tolerance(0.01) self.limb.set_goal_orientation_tolerance(0.01) def __str__(self): """ String representation of the arm. String representation of the arm, either 'left' or 'right' string will be returned. Useful when the string representation of the arm is needed, like when accessing the IK solver. """ return self._side_name def is_left(self): """Will return True if this is the left arm, false otherwise.""" return True if self._side_name == "left" else False def is_right(self): """Will return True if this is the right arm, false otherwise.""" return True if self._side_name == "right" else False def open_gripper(self): """Will open Baxter's gripper on his active hand.""" # Block ensures that the function does not return until the operation # is completed. self.gripper.open(block=True) def close_gripper(self): """Will close Baxter's gripper on his active hand.""" # Block ensures that the function does not return until the operation # is completed. self.gripper.close(block=True)
def init_mp(cls): movegroup_ns = ManipulatorActions.get_ns() robot_description = ManipulatorActions.get_robdesc_ns() sc = PlanningSceneInterface(ns=movegroup_ns) mg = MoveGroupCommander('arm', ns=movegroup_ns, robot_description=robot_description) mg.set_max_acceleration_scaling_factor(0.5) mg.set_max_velocity_scaling_factor(0.6) mg.set_end_effector_link('tool_frame') rospy.sleep(1.0) sc.remove_world_object() ManipulatorActions.rc = RobotCommander(ns=movegroup_ns) ManipulatorActions.mg = mg ManipulatorActions.sc = sc ManipulatorActions.add_table_plane() ManipulatorActions.measure_weight(calibrate=True) ManipulatorActions.move_up(home=True) ManipulatorActions.measure_weight(calibrate=True) return mg, sc
class RobotArm: """ A class used to represent the arm of a robot using both "Moveit" API and Baxter SDK API. """ def __init__(self, side_name): """ Constructor. @param side_name: which arm side we are refering to 'left' or 'right'. """ self.__side_name = side_name # MoveIt interface to a group of arm joints. # Either left arm joint group or right arm joint group. self.moveit_limb = MoveGroupCommander('{}_arm'.format(side_name)) # MoveIt limb setting. self.moveit_limb.set_end_effector_link('{}_gripper'.format(side_name)) self.moveit_limb.set_planner_id('RRTConnectKConfigDefault') self.moveit_limb.set_goal_position_tolerance(0.01) self.moveit_limb.set_goal_orientation_tolerance(0.01) # MoveIt does not provide support for Baxter gripper. # Thus we use Baxter SDK gripper instead. self.gripper = Gripper(side_name, CHECK_VERSION) self.gripper.calibrate() self.baxter_sdk_limb = Limb(side_name) def __str__(self): """ Built-in function to return a printable string representing the arm used in either print(object) or str(object). """ return str(self.__side_name) def open_gripper(self): """ Command the robot arm end-effector i.e. the gripper to open. """ self.gripper.open(block=True) def close_gripper(self): """ Command the robot arm end-effector i.e. the gripper to close. """ self.gripper.close(block=True) def is_left_arm(self): """ Return true if the arm is corresponding to the left arm. """ return self.__side_name == 'left' def is_right_arm(self): """ Return true if the arm is corresponding to the right arm. """ return self.__side_name == 'right'
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])
#!/usr/bin/env python import rospy from moveit_commander import MoveGroupCommander from sensor_msgs.msg import JointState from geometry_msgs.msg import Pose group = MoveGroupCommander("right_arm") # arm for fetch group.set_end_effector_link('r_gripper_tool_frame') group.set_pose_reference_frame('/base_footprint') def pose_callback(pose): pos = pose.position qtr = pose.orientation if qtr.x == 0 and qtr.y == 0 and qtr.z == 0 and qtr.w == 0: group.set_position_target([pos.x, pos.y, pos.z]) else: group.set_pose_target(pose) group.go() def joint_callback(msg): goal = map(lambda joint_name: msg.position[msg.name.index(joint_name)], group.get_active_joints()) group.set_joint_value_target(goal) group.go() if __name__ == "__main__": rospy.init_node('movescratch')
#!/usr/bin/env python import rospy from moveit_commander import MoveGroupCommander from sensor_msgs.msg import JointState from geometry_msgs.msg import Pose group = MoveGroupCommander("right_arm") # arm for fetch group.set_end_effector_link('r_gripper_tool_frame') group.set_pose_reference_frame('/base_footprint') def pose_callback(pose): pos = pose.position qtr = pose.orientation if qtr.x == 0 and qtr.y == 0 and qtr.z == 0 and qtr.w == 0: group.set_position_target([pos.x, pos.y, pos.z]) else: group.set_pose_target(pose) group.go() def joint_callback(msg): goal = map(lambda joint_name: msg.position[msg.name.index(joint_name)], group.get_active_joints()) group.set_joint_value_target(goal) group.go() if __name__ == "__main__": rospy.init_node('movescratch') rospy.Subscriber("movescratch/pose_goal", Pose, pose_callback) rospy.Subscriber("movescratch/joint_goal", JointState, joint_callback) rospy.spin()