def main(): init_node() # Preparing Left Arm rospy.loginfo("Preparing Left Arm...") larmg = MoveGroupCommander("left_arm") larmg.set_pose_target([0.325, 0.182, 0.067, 0.0, -math.pi / 2, 0.0]) larmg.go() # Right Arm group = MoveGroupCommander("right_arm") # Frame ID Definitoins planning_frame_id = group.get_planning_frame() tgt_frame_id = '/LARM_JOINT5_Link' # Get a target pose pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id) # Move to a point above target if pose_target: pose_target.pose.position.z += 0.4 rospy.loginfo("Set Target To: \n{}".format(pose_target)) group.set_pose_target(pose_target) ret = group.go() rospy.loginfo("Executed ... {}".format(ret)) else: rospy.logwarn("Pose Error: {}".format(pose_target))
def __init__(self): group = MoveGroupCommander("arm") #group.set_orientation_tolerance([0.3,0.3,0,3]) p = PoseStamped() p.header.frame_id = "/katana_base_link" p.pose.position.x = 0.4287 #p.pose.position.y = -0.0847 p.pose.position.y = 0.0 p.pose.position.z = 0.4492 q = quaternion_from_euler(2, 0, 0) p.pose.orientation.x = q[0] p.pose.orientation.y = q[1] p.pose.orientation.z = q[2] p.pose.orientation.w = q[3] print "Planning frame: ", group.get_planning_frame() print "Pose reference frame: ", group.get_pose_reference_frame() #group.set_pose_reference_frame("katana_base_link") print "RPy target: 0,0,0" #group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame") #group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame") group.set_pose_target(p, "katana_gripper_tool_frame") group.go() print "Current rpy: ", group.get_current_rpy( "katana_gripper_tool_frame")
def main(): init_node() # Preparing Head rospy.loginfo("Preparing Head...") headg = MoveGroupCommander("head") headg.set_joint_value_target([0.0, 60.0 / 180.0 * math.pi]) headg.go() # Preparing Both Arms rospy.loginfo("Preparing Left Arm...") barmg = MoveGroupCommander("botharms") barmg.set_pose_target([0.325, 0.482, 0.167, 0.0, -math.pi / 2, 0.0], 'LARM_JOINT5_Link') barmg.set_pose_target([0.325, -0.482, 0.167, 0.0, -math.pi / 2, 0.0], 'RARM_JOINT5_Link') barmg.go() rospy.sleep(2.0) # Right Arm group = MoveGroupCommander("right_arm") # Frame ID Definitoins planning_frame_id = group.get_planning_frame() tgt_frame_id = '/ar_marker_4' # Get a target pose pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id, 5.0) # Move to a point above target if pose_target: # Rotate Pose for Right Hand quat = [] quat.append(pose_target.pose.orientation.x) quat.append(pose_target.pose.orientation.y) quat.append(pose_target.pose.orientation.z) quat.append(pose_target.pose.orientation.w) quat = quaternion_multiply( quat, quaternion_about_axis(math.pi / 2, (1, 0, 0))) quat = quaternion_multiply( quat, quaternion_about_axis(math.pi / 2, (0, 0, 1))) pose_target.pose.orientation.x = quat[0] pose_target.pose.orientation.y = quat[1] pose_target.pose.orientation.z = quat[2] pose_target.pose.orientation.w = quat[3] pose_target.pose.position.z += 0.4 rospy.loginfo("Set Target To: \n{}".format(pose_target)) group.set_pose_target(pose_target) ret = group.go() rospy.loginfo("Executed ... {}".format(ret)) else: rospy.logwarn("Pose Error: {}".format(pose_target))
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('get_joint_states', anonymous=True) # Initialize the MoveIt! commander for the right arm arm = MoveGroupCommander('right_arm') # Get the end-effector link end_effector_link = arm.get_end_effector_link() rospy.loginfo("End effector: %s" % end_effector_link) planning_frame = arm.get_planning_frame() # Joints are stored in the order they appear in the kinematic chain joint_names = arm.get_active_joints() joint_names = [ 'right_arm_shoulder_rotate_joint', 'right_arm_shoulder_lift_joint', 'right_arm_elbow_rotate_joint', 'right_arm_elbow_bend_joint', 'right_arm_wrist_bend_joint', 'right_arm_wrist_rotate_joint' ] # Display the joint names #rospy.loginfo("Joint names:\n" + str(joint_names) + "\n") # Get the current joint angles joint_values = arm.get_current_joint_values() # Display the joint values rospy.loginfo("Joint values:\n" + str(joint_values) + "\n") # Get the end-effector pose ee_pose = arm.get_current_pose(end_effector_link) orientation = ee_pose.pose.orientation ox = orientation.x oy = orientation.y oz = orientation.z ow = orientation.w euler_pose = euler_from_quaternion([ow, ox, oy, oz]) #euler_pose = euler_from_quaternion([0.0, 0.0, 0.0, 1.0]) # Display the end-effector pose rospy.loginfo("End effector pose:\n" + str(ee_pose)) rospy.loginfo("RPY?:\n" + str(euler_pose)) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_move_position', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) arm.set_planning_time(10) arm.set_named_target(START_POSITION) arm.go() rospy.sleep(2) print "======== create new goal position ========" start_pose = PoseStamped() start_pose.header.frame_id = arm.get_planning_frame() # Test Position start_pose.pose.position.x = 0.2 # 20 cm start_pose.pose.position.y = -0.11 # -11 cm start_pose.pose.position.z = 0.6 # 60 cm q = quaternion_from_euler(0.0, 0.0, 0.0) start_pose.pose.orientation = Quaternion(*q) print start_pose print "====== move to position =======" # KDL # arm.set_joint_value_target(start_pose, True) # IK Fast arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z], arm.get_end_effector_link()) arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_test_position', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) # max planning time arm.set_planning_time(10) # start pose arm.set_start_state_to_current_state() end_effector = arm.get_end_effector_link() rospy.sleep(1) print "======== create 100 new goal position ========" start_pose = PoseStamped() start_pose.header.frame_id = arm.get_planning_frame() i = 1 while i <= 1000: # random position start_pose = arm.get_random_pose(end_effector) q = quaternion_from_euler(0.0, 0.0, 0.0) start_pose.pose.orientation = Quaternion(*q) print "====== move to position", i, "=======" # KDL arm.set_joint_value_target(start_pose, True) # IK Fast # arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, # start_pose.pose.position.z], end_effector) i += 1 arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def change_tray_model(scene, onigiri_offset): botharms = MoveGroupCommander("botharms") box_pose = PoseStamped() box_pose.header.frame_id = botharms.get_planning_frame() scene.remove_world_object("tray") rospack = rospkg.RosPack() resourcepath = rospack.get_path( 'khi_duaro_onigiri_system') + "/config/meshes/" rot_o = 0.0 / 180.0 * math.pi rot_a = 0.0 / 180.0 * math.pi rot_t = 0.0 / 180.0 * math.pi q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx') if onigiri_offset == True: meshpath = resourcepath + "tray.stl" zoffset = 0.02 xoffset = 0.0085 yoffset = -0.005 else: meshpath = resourcepath + "tray_nooffset.stl" zoffset = 0.0 xoffset = 0.0 yoffset = 0.0 #setting origin of tray box_pose.pose.position.x = -0.58952 + xoffset box_pose.pose.position.y = -0.0613 + yoffset box_pose.pose.position.z = 0.6375 + zoffset box_pose.pose.orientation.x = q[0] box_pose.pose.orientation.y = q[1] box_pose.pose.orientation.z = q[2] box_pose.pose.orientation.w = q[3] scene.add_mesh('tray', box_pose, meshpath, (1, 1, 1)) rospy.sleep(1)
def __init__(self): group = MoveGroupCommander("arm") #group.set_orientation_tolerance([0.3,0.3,0,3]) p = PoseStamped() p.header.frame_id = "/katana_base_link" p.pose.position.x = 0.4287 #p.pose.position.y = -0.0847 p.pose.position.y = 0.0 p.pose.position.z = 0.4492 q = quaternion_from_euler(2, 0, 0) p.pose.orientation.x = q[0] p.pose.orientation.y = q[1] p.pose.orientation.z = q[2] p.pose.orientation.w = q[3] print "Planning frame: " ,group.get_planning_frame() print "Pose reference frame: ",group.get_pose_reference_frame() #group.set_pose_reference_frame("katana_base_link") print "RPy target: 0,0,0" #group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame") #group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame") group.set_pose_target(p, "katana_gripper_tool_frame") group.go() print "Current rpy: " , group.get_current_rpy("katana_gripper_tool_frame")
pose_target_1.position.y = -0.1 pose_target_1.position.z = 0.15 pose_target_1.orientation.x = 0.0 pose_target_1.orientation.y = -0.707 pose_target_1.orientation.z = 0.0 pose_target_1.orientation.w = 0.707 rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_1)) group.set_pose_target(pose_target_1) group.go() # Add Object to the Planning Scene rospy.loginfo("Add Objects to the Planning Scene...") box_pose = PoseStamped() box_pose.header.frame_id = group.get_planning_frame() box_pose.pose.position.x = 0.3 box_pose.pose.position.y = -0.3 box_pose.pose.position.z = -0.25 box_pose.pose.orientation.w = 1.0 scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5)) rospy.sleep(2) rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names())) # Set Path Constraint constraints = Constraints() constraints.name = "down" orientation_constraint = OrientationConstraint()
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.detected_names = rospy.get_param( '/darknet_ros/yolo_model/detection_classes/names') self.object_pose_sub = rospy.Subscriber( '/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) self.listener = tf.TransformListener() self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.position_list = [] self.orientation_list = [] self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.5) self.robot_arm.set_planning_time(10) self.robot_arm.set_num_planning_attempts(10) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): ''' planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print"====== current pose(1) : ", self.wpose ''' marker_joint_goal = [ 4.721744537353516, -0.7451499144183558, -1.6199515501605433, -1.2175200621234339, 1.6366002559661865, 3.1263363361358643 ] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) ''' self.wpose = self.robot_arm.get_current_pose() print"====== current pose(2) : ", self.wpose ''' def move_code2(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print "====== current pose(1) : ", self.wpose marker_joint_goal = [ 4.768651485443115, -1.1165898481952112, -2.1672890822040003, 5.898628234863281, 1.7003079652786255, 3.2297513484954834 ] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) self.wpose = self.robot_arm.get_current_pose() print "====== current pose(2) : ", self.wpose def plan_cartesian_path(self, scale=1.0): waypoints = [] ''' self.wpose = self.robot_arm.get_current_pose() print"====== current pose(1) : ", self.wpose ''' self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x += scale * 0.009 self.wpose.position.y += scale * 0.07 self.wpose.position.z -= scale * 0.06 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) ''' self.wpose = self.robot_arm.get_current_pose() print"====== current pose(2) : ", self.wpose ''' return plan, fraction def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') detection_start = time.time() rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) print("detection time", format(time.time() - detection_start)) detection_start = time.time() def collectJsk(self, msg): # detection_start = time.time() try: detection_start = time.time() (trans, rot) = self.listener.lookupTransform('base_link', 'yolo_output00', rospy.Time(0)) self.goal_x = trans[0] self.goal_y = trans[1] self.goal_z = trans[2] self.goal_ori_x = rot[0] self.goal_ori_y = rot[1] self.goal_ori_z = rot[2] self.goal_ori_w = rot[3] self.position_list = [self.goal_x, self.goal_y, self.goal_z] self.orientation_list = [ self.goal_ori_x, self.goal_ori_y, self.goal_ori_z, self.goal_ori_w ] (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.orientation_list) except: return print("detection time", format(time.time() - detection_start)) # print("detection time",format(time.time()-detection_start)) def go_to_move(self, scale=1.0): ''' planning_frame = self.robot_arm.get_planning_frame() self.wpose = self.robot_arm.get_current_pose() print"====== current pose(1) : ", self.wpose ''' self.calculated_tomato.position.x = (scale * self.goal_x) self.calculated_tomato.position.y = (scale * self.goal_y) self.calculated_tomato.position.z = (scale * self.goal_z) self.calculated_tomato.orientation = Quaternion( *quaternion_from_euler(3.14, 0, 1.57)) print("============ tomato_pose goal frame: ", self.calculated_tomato) self.robot_arm.set_pose_target(self.calculated_tomato) tf_display_position = [ self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z ] tf_display_orientation = [ self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato)) (tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path( tomato_waypoints, 0.01, 0.0) self.display_trajectory(tomato_plan) print("======= Press `Enter` to if plan in correct!======") raw_input() self.robot_arm.go(True) ''' self.wpose = self.robot_arm.get_current_pose() print"====== current pose(2) : ", self.wpose ''' def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory)
pose_target_1.orientation.z = 0.0 pose_target_1.orientation.w = 0.707 rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_1)) group.set_pose_target(pose_target_1) group.go() # Add Object to Planning Scene rospy.loginfo("Planning Scene Settings") scene = PlanningSceneInterface() rospy.sleep(2) # Waiting for PlanningSceneInterface box_pose = PoseStamped() box_pose.header.frame_id = group.get_planning_frame() box_pose.pose.position.x = 0.3 box_pose.pose.position.y = -0.3 box_pose.pose.position.z = -0.25 box_pose.pose.orientation.w = 1.0 scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5)) rospy.sleep(2) rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names())) # Pose Target 2 rospy.loginfo("Start Pose Target 2") pose_target_2 = Pose() pose_target_2.position.x = 0.3 pose_target_2.position.y = -0.5
class BaxterMoveit(BaxterRobot): LEFT = -1 RIGHT = 1 def __init__(self, node, limb=+1): super(BaxterMoveit, self).__init__(node, limb=+1) self.group = MoveGroupCommander(self.lns + "_arm") self.robot = RobotCommander() self.scene = PlanningSceneInterface() self._info() def _info(self): '''Getting Basic Information''' # name of the reference frame for this robot: print("@@@@@@@@@@@@ Reference frame: %s" % self.group.get_planning_frame()) # We can also print the name of the end-effector link for this group: print("@@@@@@@@@@@@ End effector: %s" % self.group.get_end_effector_link()) # We can get a list of all the groups in the robot: print("@@@@@@@@@@@@ Robot Groups: @@@@@@@@@@@@@", self.robot.get_group_names()) # Sometimes for debugging it is useful to print the entire state of the # robot: print("@@@@@@@@@@@@ Printing robot state @@@@@@@@@@@@@") print(self.robot.get_current_state()) def obstacle(self, name, position, size): planning_frame = self.robot.get_planning_frame() pose = PoseStamped() pose.header.frame_id = planning_frame pose.pose.position.x = position[0] pose.pose.position.y = position[1] pose.pose.position.z = position[2] self.scene.add_box(name, pose, tuple(size)) def movej(self, q, raw=False): ''' move in joint space by giving a joint configuration as dictionary''' if raw: print("in moveit 'raw' motion is not avaiable") # succ = False # while succ is False: succ = self.group.go(q, wait=True) # self.group.stop() # ensures that there is no residual movement def showplan(self, target): if type(target) is PyKDL.Frame or type(target) is Pose: q = self.ik(target) elif type(target) is dict: q = target else: print("Target format error") return self.group.set_joint_value_target(q) self.group.plan() def movep(self, pose, raw=False): ''' move the eef in Cartesian space by giving a geometry_msgs.Pose or a PyKDL.Frame''' if type(pose) is PyKDL.Frame: pose = transformations.KDLToPose(pose) q = self.ik(pose) if q is not None: self.movej(q, raw=raw) else: print("\nNO SOLUTION TO IK\n" * 20)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.detected_names = rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') self.object_pose_sub = rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk) self.listener = tf.TransformListener() # self.object_name_sub = rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect) self.tomatoboundingBox = BoundingBox() display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20) global goal_x global goal_y global goal_z global goal_roll global goal_pitch global goal_yaw self.distance = 0 self.trans = [0.0, 0.0, 0.0] # self.marker = [] self.tomato = [] self.position_list = [] self.orientation_list = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print ("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print ("====== current pose : ", self.wpose) joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982] print ("INIT POSE: ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait = True) def go_to_desired_coordinate(self): self.calculated_tomato.position.x = goal_x self.calculated_tomato.position.y = goal_y self.calculated_tomato.position.z = goal_z self.calculated_tomato.orientation.x = goal_roll self.calculated_tomato.orientation.y = goal_pitch self.calculated_tomato.orientation.z = goal_yaw tf_display_position = [self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z] tf_display_orientation = [self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w] ii = 0 while ii < 5: ii += 1 self.br.sendTransform( tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz tomato_id_goal_waypoints = [] tomato_id_goal_waypoints.append(copy.deepcopy(calculated_tomato)) (tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(tomato_id_goal_waypoints, 0.01, 0.0) self.display_trajectory(tomato_id_goal_plan) print ("============ Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk) # rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect) def collectJsk(self, msg): global goal_x global goal_y global goal_z global goal_roll global goal_pitch global goal_yaw try: (trans, rot) = self.listener.lookupTransform('base_link','yolo_output00', rospy.Time(0)) goal_x = round(trans[0],2) goal_y = round(trans[1],2) goal_z = round(trans[2],2) print("====== trans [x, y, z]: ", trans) print("====== rotat [x, y, z, w]: ", rot) orientation = euler_from_quaternion(rot) goal_roll = orientation[0] goal_pitch = orientation[1] goal_yaw = orientation[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn('there is no tf')
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.detected_names = rospy.get_param( '/darknet_ros/yolo_model/detection_classes/names') self.object_pose_sub = rospy.Subscriber( '/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) self.listener = tf.TransformListener() display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.ii = 0 global goal_x global goal_y global goal_z global ori_w global ori_x global ori_y global ori_z self.distance = 0 self.tomato = [] self.position_list = [] self.orientation_list = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.calculated_tomato_coor = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(10) self.robot_arm.set_num_planning_attempts(10) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print "====== current pose : ", self.wpose marker_joint_goal = [ 2.6482303142547607, -0.3752959410296839, -2.1118043104754847, -0.4801228682147425, -1.4944542090045374, -4.647655431424276 ] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.get_param('/darknet_ros/yolo_model/detection_classes/names') rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray, self.collectJsk) def go_to_goal_point(self, scale=1.0): planning_frame = self.robot_arm.get_planning_frame() print(">> robot arm planning frame: \n", planning_frame) self.calculated_tomato.position.x = (scale * goal_x) self.calculated_tomato.position.y = (scale * goal_y) self.calculated_tomato.position.z = (scale * goal_z) self.calculated_tomato.orientation.w = (scale * ori_w) self.calculated_tomato.orientation.x = (scale * ori_x) self.calculated_tomato.orientation.y = (scale * ori_y) self.calculated_tomato.orientation.z = (scale * ori_z) print(">> robot_pose goal frame: ", self.calculated_tomato) self.robot_arm.set_pose_target(self.calculated_tomato) tf_display_position = [ self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z ] tf_display_orientation = [ self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato)) (tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path( tomato_waypoints, 0.01, 0.0) self.display_trajectory(tomato_plan) print("======= Press `Enter` to if plan in correct!======") raw_input() self.robot_arm.go(True) ''' def go_to_designated_coordinate(self): desired_goal_pose = [-0.537,0.166, 0.248] Cplanning_frame = self.robot_arm.get_planning_frame() print(">> current planning frame: \n",Cplanning_frame) self.calculated_tomato_coor.position.x = desired_goal_pose[0] self.calculated_tomato_coor.position.y = desired_goal_pose[1] self.calculated_tomato_coor.position.z = desired_goal_pose[2] self.calculated_tomato_coor.orientation = Quaternion(*quaternion_from_euler(desired_goal_pose[0],desired_goal_pose[1],desired_goal_pose[2])) print(">> goal frame", self.calculated_tomato_coor) self.robot_arm.set_pose_target(self.calculated_tomato_coor) tf_display_position = [self.calculated_tomato_coor.position.x, self.calculated_tomato_coor.position.y, self.calculated_tomato_coor.position.z] tf_display_orientation = [self.calculated_tomato_coor.orientation.x, self.calculated_tomato_coor.orientation.y, self.calculated_tomato_coor.orientation.z, self.calculated_tomato_coor.orientation.w] jj = 0 while jj < 5: jj += 1 self.br.sendTransform( tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato_coor)) (plan, fraction) = self.robot_arm.compute_cartesian_path(tomato_waypoints,0.01,0.0) self.display_trajectory(plan) print("=========== Press `Enter` to if plan is correct!!...") raw_input() self.robot_arm.go(True) ''' def plan_cartesian_path(self, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x -= scale * 0.1 self.wpose.position.y += scale * 0.1 waypoints.append(copy.deepcopy(self.wpose)) ''' self.wpose.position.x -= scale*0.2 waypoints.append(copy.deepcopy(self.wpose)) ''' ''' self.wpose.position.x -= scale*0.1 waypoints.append(copy.deepcopy(self.wpose)) ''' (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def collectJsk(self, msg): global goal_x global goal_y global goal_z global ori_w global ori_x global ori_y global ori_z try: (trans, rot) = self.listener.lookupTransform('base_link', 'yolo_output00', rospy.Time(0)) goal_x = trans[0] goal_y = trans[1] goal_z = trans[2] ori_w = rot[0] ori_x = rot[1] ori_y = rot[2] ori_z = rot[3] print("==== trans[x,y,z]: ", trans) print("==== rot[x,y,z,w]: ", rot) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logwarn('there is no tf')
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.detected = {} self.listener = tf.TransformListener() display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20) self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.position_list = [] self.orientation_list = [] self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.5) self.robot_arm.set_planning_time(10) self.robot_arm.set_num_planning_attempts(10) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print"====== current pose(1) : ", self.wpose marker_joint_goal = [4.721744537353516, -0.7451499144183558, -1.6199515501605433, -1.2175200621234339, 1.6366002559661865, 3.1263363361358643] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) self.wpose = self.robot_arm.get_current_pose() print"====== current pose(2) : ", self.wpose def execute_plan(self,plan): group = self.robot_arm group.execute(plan, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory)
if __name__=='__main__': rospy.init_node("cartesianpathtest") rospy.loginfo("Starting up move group commander for right arm") right_arm_mgc = MoveGroupCommander("right_arm_torso") #init_pose = right_arm_mgc.get_current_pose() init_pose = Pose(Point(0.4, -0.2, 1.2),Quaternion(0.0,0.0,0.0,1.0)) goal_point = Point(0.45, -0.2, 1.4) goal_ori = Quaternion(0.0,0.0,0.0,1.0) goal_pose = Pose(goal_point, goal_ori) right_arm_mgc.set_pose_reference_frame('base_link') rospy.loginfo("Planning on frame:" + str(right_arm_mgc.get_planning_frame())) waypoints = [] waypoints.append(init_pose)#.pose) waypoints.append(goal_pose) eef_step_max = 0.05 jump_thresh_max = 10000 while True: curr_eef_step = eef_step_max * random() curr_jump_thresh = jump_thresh_max * random() #path, fraction = right_arm_mgc.compute_cartesian_path(waypoints, curr_eef_step, curr_jump_thresh, False) path, fraction = right_arm_mgc.compute_cartesian_path(waypoints, 0.01, 1.2, False) rospy.loginfo("Fraction and path:") rospy.loginfo(str(fraction)) if fraction <= 0:
class LinkProxy(): def __init__(self, arm): self._arm = arm self._group_name = None self._group = None self._planner = None self.orientation_frozen = False def _initialize_group(self): # get list of planners available to this move group available_planners = rospy.get_param( "/move_group/%s/planner_configs" % self._group_name, None) if available_planners is None: ROS_ERR("The Move group `%s` does not have planners configured.", self._group_name) return False if self._planner not in available_planners: ROS_WARN( "The planner `%s` is not available for the group `%s`, using `%s` instead.", self._planner, self._group_name, available_planners[0]) self._planner = available_planners[0] self._group = MoveGroupCommander(self._group_name) # set planner self._group.set_planner_id(self._planner) def _is_group_valid(self): if self._group is None: if isinstance(self, TerminalLink) or isinstance( self, IntermediateLink): ROS_ERR("Link object was created without a group") return False else: ROS_ERR("Cannot instantiate abstract classes") return False return True def plan_to_pose(self, pose, *args): raise NotImplentedError() def plan_cartesian_path(self, waypoints, *args): raise NotImplentedError() def execute_plan(self, plan): if self._is_group_valid(): return self._arm._execute_plan(self._group, plan) return False def get_pose(self): return self._group.get_current_pose().pose def is_at(self, pose, epsilon): cur_pose = self.get_pose() cur_position = [ cur_pose.position.x, cur_pose.position.y, cur_pose.position.z ] goal_position = [pose.position.x, pose.position.y, pose.position.z] eucl_dist = np.linalg.norm( np.array(cur_position) - np.array(goal_position)) return eucl_dist < epsilon def get_planning_frame(self): if self._is_group_valid(): return self._group.get_planning_frame() return False def freeze_orientation(self): # get current pose cur_pose = self._group.get_current_pose() # create pose constraint constraints = Constraints() constraints.name = "orientation_constraints" orientation_constraint = OrientationConstraint() orientation_constraint.header = cur_pose.header orientation_constraint.header.stamp = rospy.Time.now() orientation_constraint.link_name = self._group.get_end_effector_link() orientation_constraint.orientation = cur_pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 0.5 orientation_constraint.absolute_y_axis_tolerance = 0.5 orientation_constraint.absolute_z_axis_tolerance = 0.5 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) self._group.set_path_constraints(constraints) self.orientation_frozen = True def clear_path_constraints(self): self._group.set_path_constraints(None) self.orientation_frozen = False
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) #self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.marker = [] self.position_list = [] self.orientation_list = [] self.m_idd = 0 self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.ar_pose = Pose() self.goalPoseFromAR = Pose() self.br = tf.TransformBroadcaster() #self.goalPose_from_arPose = Pose() self.trans = [] self.rot = [] self.target_ar_id = 9 self.calculed_coke_pose = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): """ move to initial pose of the UR3 """ #self.clear_octomap() planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print "====== current pose : ", self.wpose marker_joint_goal = [ -0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982 ] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) def move_moveit_setting_pose(self, pose_name): """ move to one of the moveit_setup pose: "home", "zeros", "table" """ if pose_name == "home": self.robot_arm.set_named_target("home") elif pose_name == "zeros": self.robot_arm.set_named_target("zeros") elif pose_name == "table": self.robot_arm.set_named_target("table") #print "Press the Enter" #raw_input() self.robot_arm.go(wait=True) def go_to_move(self, scale=1.0): """ manipulator is moving to the target ar_marker. \n the start ar_marker id is 9, goal ar_marker id is 10. """ #self.calculed_coke_pose = self.robot_arm.get_current_pose() planning_frame = self.robot_arm.get_planning_frame() coke_offset = [0, -0.35, -0.1] #x y z # gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품. # linear offset = abs([0, 0.0625, 0.13]) robot_base_offset = 0.873 base_wrist2_offset = 0.1 #for avoiding link contact error if self.target_ar_id == 9: print ">> robot arm plannig frame: \n", planning_frame print ">> move mode id: ", self.target_ar_id self.calculed_coke_pose.position.x = ( scale * self.goal_x) # base_link to wrist2 x-offset self.calculed_coke_pose.position.y = (scale * self.goal_y) + coke_offset[1] #self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset self.calculed_coke_pose.position.z = ( scale * self.goal_z ) + robot_base_offset # world to base_link z-offset and coke can offset self.calculed_coke_pose.orientation = Quaternion( *quaternion_from_euler(3.14, 0, 1.57)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) elif self.target_ar_id == 10: print ">> robot arm plannig frame: \n", planning_frame print ">> move mode id: ", self.target_ar_id self.calculed_coke_pose.position.x = (scale * self.goal_x) + coke_offset[1] self.calculed_coke_pose.position.y = (scale * self.goal_y) + 0 self.calculed_coke_pose.position.z = ( scale * self.goal_z ) + robot_base_offset # world to base_link z-offset and coke can offset self.calculed_coke_pose.orientation = Quaternion( *quaternion_from_euler(3.14, 0, 0)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) tf_display_position = [ self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z ] tf_display_orientation = [ self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz coke_waypoints = [] coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose)) (coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path( coke_waypoints, 0.01, 0.0) self.display_trajectory(coke_plan) ## ## ## print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch, yaw): """ Manipulator is moving to the desired coordinate. \n Now move to the ar_10 marker. """ calculed_ar_id_10 = Pose() #desired_goal_pose = [0.171, -0.113, 1.039] #desired_goal_euler = [3.14, 0.17, 0] desired_goal_pose = [pose_x, pose_y, pose_z] desired_goal_euler = [roll, pitch, yaw] Cplanning_frame = self.robot_arm.get_planning_frame() print ">> current planning frame: \n", Cplanning_frame calculed_ar_id_10.position.x = desired_goal_pose[0] + 0.1 calculed_ar_id_10.position.y = desired_goal_pose[1] calculed_ar_id_10.position.z = desired_goal_pose[2] calculed_ar_id_10.orientation = Quaternion(*quaternion_from_euler( desired_goal_euler[0], desired_goal_euler[1], desired_goal_euler[2])) print ">>> ar id 10 goal frame: ", desired_goal_pose self.robot_arm.set_pose_target(calculed_ar_id_10) tf_display_position = [ calculed_ar_id_10.position.x, calculed_ar_id_10.position.y, calculed_ar_id_10.position.z ] tf_display_orientation = [ calculed_ar_id_10.orientation.x, calculed_ar_id_10.orientation.y, calculed_ar_id_10.orientation.z, calculed_ar_id_10.orientation.w ] jj = 0 while jj < 5: jj += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz ar_id_10_waypoints = [] ar_id_10_waypoints.append(copy.deepcopy(calculed_ar_id_10)) (ar_id_10_plan, ar_id_10_fraction) = self.robot_arm.compute_cartesian_path( ar_id_10_waypoints, 0.01, 0.0) self.display_trajectory(ar_id_10_plan) ## ## ## print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): """ Visualized trajectory of the manipulator """ display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0): """ Working on the "linear move": x-axis -> y-axis -> z-axis """ waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 #print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # waypoints.append(copy.deepcopy(self.wpose)) ii += 1 #print "waypoints:", waypoints (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): """ Working on the "linear move": x-axis """ waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): """ Working on the "linear move": y-axis """ waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset #-0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): """ Working on the "linear move": z-axis """ waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose #print "===== robot arm pose: ", self.wpose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset #-0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): """ execute planned "plan" trajectory. """ group = self.robot_arm group.execute(plan, wait=True) def ar_pose_subscriber(self): """ ar_maker subscriber """ rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") #print "======= pos(meter): ", self.position_list #print "======= orientation: ", self.orientation_list def ar_tf_listener(self, msg): """ Listening the ar_marker pose coordinate. """ try: self.marker = msg.markers ml = len(self.marker) target_start_point_id = self.target_ar_id #target_id = target_ar_id #self.m_idd = self.marker[0].id # 임시용 for ii in range(0, ml): # 0 <= ii < ml self.m_idd = self.marker[ii].id #print "checked all id: ", self.m_idd if self.m_idd != target_start_point_id: pass #target_id_flage = False elif self.m_idd == target_start_point_id: target_id_flage = True target_id = self.m_idd target_id_index = ii #print "target id: ", target_id_index, target_id, target_id_flage if target_id_flage == True: self.ar_pose.position.x = self.marker[ target_id_index].pose.pose.position.x self.ar_pose.position.y = self.marker[ target_id_index].pose.pose.position.y self.ar_pose.position.z = self.marker[ target_id_index].pose.pose.position.z self.ar_pose.orientation.x = self.marker[ target_id_index].pose.pose.orientation.x self.ar_pose.orientation.y = self.marker[ target_id_index].pose.pose.orientation.y self.ar_pose.orientation.z = self.marker[ target_id_index].pose.pose.orientation.z self.ar_pose.orientation.w = self.marker[ target_id_index].pose.pose.orientation.w self.goal_x = self.ar_pose.position.x self.goal_y = self.ar_pose.position.y self.goal_z = self.ar_pose.position.z self.position_list = [self.goal_x, self.goal_y, self.goal_z] self.orientation_list = [ self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w ] (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion( self.orientation_list) #list form으로 넘겨주어야 함 #print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z #print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw #print "ar_pos(meter): \n", self.position_list #print "ar_orientation: \n", self.orientation_list except: return
import rospy from moveit_commander import RobotCommander, MoveGroupCommander from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown from geometry_msgs.msg import PoseStamped, Pose from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation from trajectory_msgs.msg import JointTrajectoryPoint rospy.init_node("motion_planning") scene = PlanningSceneInterface() robot = RobotCommander() L_arm = MoveGroupCommander("L_arm") rospy.sleep (1) #print (robot.get_current_variable_values()) print (L_arm.get_current_pose()) print (L_arm.get_planning_frame()) L_arm.set_planning_time(10); L_arm.set_pose_reference_frame("omo_L") L_arm.set_position_target([-0.068, -0.120, 0.520]) print (L_arm.get_end_effector_link()) plan = L_arm.plan() print (plan) L_arm.execute(plan) print (L_arm.get_current_pose(L_arm.get_end_effector_link()))
#GROUP_NAME_GRIPPER = "NAME OF GRIPPER" roscpp_initialize(sys.argv) rospy.init_node('joint_control_UR', anonymous=True) robot = RobotCommander() scene = PlanningSceneInterface() group_name = "ur3_manipulator" move_group = MoveGroupCommander(group_name) FIXED_FRAME = 'world' display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', DisplayTrajectory, queue_size=20) # We can get the name of the reference frame for this robot: planning_frame = move_group.get_planning_frame() print "============ Planning frame: %s" % planning_frame # We can also print the name of the end-effector link for this group: eef_link = move_group.get_end_effector_link() print "============ End effector link: %s" % eef_link # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print "============ Available Planning Groups:", robot.get_group_names() # Sometimes for debugging it is useful to print the entire state of the # robot: print "============ Printing robot state" print robot.get_current_state() print ""
scene.add_sphere("ball", p, 0.1) p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = -0.1 scene.add_box("table", p, (1.0, 2.6, 0.2)) p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = -0.4 scene.add_plane("ground_plane", p) rospy.sleep(20) ## We can get the name of the reference frame for this robot print ">>>>> Reference frame: %s" % group.get_planning_frame() print ">>>>> Printing robot state" print robot.get_current_state() print ">>>>> Printing robot pose" print group.get_current_pose() ## Planning to a Pose goal print ">>>>> Generating plan" pose_target = geometry_msgs.msg.Pose() #pose_target.orientation.w = 1.0 pose_target.position.x = 0.5 #0.4 pose_target.position.y = 0.2 #0.2 pose_target.position.z = 0.2 #0.2 group.set_pose_target(pose_target)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move',anonymous=True) rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) #self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.marker = [] self.position_list = [] self.orientation_list = [] self.m_idd = 0 self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.ar_pose = Pose() self.goalPoseFromAR = Pose() self.br = tf.TransformBroadcaster() #self.goalPose_from_arPose = Pose() self.trans = [] self.rot = [] self.calculed_coke_pose = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): self.clear_octomap() planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print"====== current pose : ", self.wpose marker_joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982] print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position self.robot_arm.go(marker_joint_goal, wait=True) def look_object(self): look_object = self.robot_arm.get_current_joint_values() ## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함 print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5] #look_object[5] = math.radians(90) look_object[5] = 0 ''' if look_object[5] > abs(math.radians(180)): if look_object[5] < 0: look_object[5] = look_object[5] + math.radians(90) # wrist3 elif look_object[5] > 0: look_object[5] = look_object[5] - math.radians(90) else: look_object[5] = look_object[5] - math.radians(90) # wrist3 ''' print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5] #look_object[3] = look_object[3] - (math.radians(00)) # wrist1 self.robot_arm.go(look_object, wait=True) #look_object[5] = look_object[5] + (math.radians(90)) # wrist3 #self.robot_arm.go(look_object, wait=True) def look_up_down(self): self.clear_octomap() print "======== clear_octomap... Please wait...." look_up_down = self.robot_arm.get_current_joint_values() #print "before: ", look_up_down look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory); def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 #print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # waypoints.append(copy.deepcopy(self.wpose)) ii += 1 print "waypoints:", waypoints (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def print_ar_pose(self): rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") print "======= pos(meter): ", self.position_list print "======= orientation: ", self.orientation_list def go_to_move(self, scale = 1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기 #self.calculed_coke_pose = self.robot_arm.get_current_pose() planning_frame = self.robot_arm.get_planning_frame() coke_offset = [0, -0.35, -0.1] #x y z # gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품. # linear offset = abs([0, 0.0625, 0.13]) robot_base_offset = 0.873 base_wrist2_offset = 0.1 #for avoiding link contact error print "========== robot arm plannig frame: \n", planning_frame self.calculed_coke_pose.position.x = (scale * self.goal_x) + base_wrist2_offset # base_link to wrist2 x-offset self.calculed_coke_pose.position.y = (scale * self.goal_y) + coke_offset[1] #self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset self.calculed_coke_pose.position.z = (scale * self.goal_z) + robot_base_offset # world to base_link z-offset and coke can offset self.calculed_coke_pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.54)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) tf_display_position = [self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z] tf_display_orientation = [self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w] ii = 0 while ii < 5: ii += 1 self.br.sendTransform( tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz coke_waypoints = [] coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose)) (coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path(coke_waypoints, 0.01, 0.0) self.display_trajectory(coke_plan) ## ## ## print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def ar_tf_listener(self, msg): try: self.marker = msg.markers ml = len(self.marker) target_id = 9 #self.m_idd = self.marker[0].id # 임시용 for ii in range(0, ml): # 0 <= ii < ml self.m_idd = self.marker[ii].id #print "checked all id: ", self.m_idd if self.m_idd != target_id: pass #target_id_flage = False elif self.m_idd == target_id: target_id_flage = True target_id = self.m_idd target_id_index = ii #print "target id: ", target_id_index, target_id, target_id_flage if target_id_flage == True: self.ar_pose.position.x = self.marker[target_id_index].pose.pose.position.x self.ar_pose.position.y = self.marker[target_id_index].pose.pose.position.y self.ar_pose.position.z = self.marker[target_id_index].pose.pose.position.z self.ar_pose.orientation.x = self.marker[target_id_index].pose.pose.orientation.x self.ar_pose.orientation.y = self.marker[target_id_index].pose.pose.orientation.y self.ar_pose.orientation.z = self.marker[target_id_index].pose.pose.orientation.z self.ar_pose.orientation.w = self.marker[target_id_index].pose.pose.orientation.w self.goal_x = self.ar_pose.position.x self.goal_y = self.ar_pose.position.y self.goal_z = self.ar_pose.position.z self.position_list = [self.goal_x, self.goal_y, self.goal_z] self.orientation_list = [self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w] (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.orientation_list) #list form으로 넘겨주어야 함 #print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z #print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw #print "ar_pos(meter): \n", self.position_list #print "ar_orientation: \n", self.orientation_list except: return
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) #rospy.init_node('moveit_demo') # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) #cartesian = rospy.get_param('~cartesian', True) print "===== It is OK ====" rospy.sleep(3) # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=1) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the left arm left_arm = MoveGroupCommander('left_arm') left_gripper = MoveGroupCommander('left_gripper') # Get the name of the end-effector link left_eef = left_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) left_arm.set_goal_position_tolerance(0.01) left_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution left_arm.allow_replanning(True) left_reference_frame = left_arm.get_planning_frame() # Set the left arm reference frame left_arm.set_pose_reference_frame('base') # Allow 5 seconds per planning attempt left_arm.set_planning_time(10) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # Set a limit on the number of place attempts max_place_attempts = 10 # Give the scene a chance to catch up rospy.sleep(2) #object1_id = 'object1' table_id = 'table' target_id = 'target' #tool_id = 'tool' #obstacle1_id = 'obstacle1' # Remove leftover objects from a previous run #scene.remove_world_object(object1_id) scene.remove_world_object(table_id) scene.remove_world_object(target_id) #scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object('base', target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file left_arm.set_named_target('left_arm_zero') left_arm.go() rospy.sleep(1) left_gripper.set_joint_value_target(GRIPPER_OPEN) left_gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.0 #object1_size = [0.088, 0.04, 0.02] # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene #obstacle1_size = [0.3, 0.05, 0.45] # Add a table top and two boxes to the scene #obstacle1_pose = PoseStamped() #obstacle1_pose.header.frame_id = left_reference_frame #obstacle1_pose.pose.position.x = 0.96 #obstacle1_pose.pose.position.y = 0.24 #obstacle1_pose.pose.position.z = 0.04 #obstacle1_pose.pose.orientation.w = 1.0 #scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) #self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0) #object1_pose = PoseStamped() #object1_pose.header.frame_id = left_reference_frame #object1_pose.pose.position.x = 0.80 #object1_pose.pose.position.y = 0.04 #object1_pose.pose.position.z = table_ground + table_size[2] + object1_size[2] / 2.0 #object1_pose.pose.orientation.w = 1.0 #scene.add_box(object1_id, object1_pose, object1_size) # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = left_reference_frame table_pose.pose.position.x = 1 table_pose.pose.position.y = 0.7 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = left_reference_frame target_pose.pose.position.x = 1 target_pose.pose.position.y = 0.7 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange #self.setColor(object1_id, 0.8, 0, 0, 1.0) self.setColor(table_id, 0.8, 0, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object left_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = left_reference_frame place_pose.pose.position.x = 0.18 place_pose.pose.position.y = -0.18 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it #grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = left_arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = left_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file left_arm.set_named_target('left_arm_zero') left_arm.go() # Open the gripper to the neutral position left_gripper.set_joint_value_target(GRIPPER_OPEN) left_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def main(): rospy.init_node('duaro') botharms = MoveGroupCommander("botharms") upper = MoveGroupCommander("upper_arm") lower = MoveGroupCommander("lower_arm") duaro = RobotCommander() # Add Object to Planning Scene rospy.loginfo("Planning Scene Settings") scene = PlanningSceneInterface() rospy.sleep(2) # Waiting for PlanningSceneInterface scene.remove_world_object() current_robot = duaro.get_current_state() current_joint = current_robot.joint_state.position rospack = rospkg.RosPack() resourcepath = rospack.get_path( 'khi_duaro_onigiri_system') + "/config/meshes/" #conveyer box_pose = PoseStamped() box_pose.header.frame_id = botharms.get_planning_frame() rot_o = 180.0 / 180.0 * math.pi rot_a = 0.0 / 180.0 * math.pi rot_t = 0.0 / 180.0 * math.pi q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx') #setting origin of conveyer box_pose.pose.position.x = 0.2153 box_pose.pose.position.y = 0.32454 box_pose.pose.position.z = 0.0 box_pose.pose.orientation.x = q[0] box_pose.pose.orientation.y = q[1] box_pose.pose.orientation.z = q[2] box_pose.pose.orientation.w = q[3] meshpath = resourcepath + "conveyer.stl" scene.add_mesh('conveyer', box_pose, meshpath, (1, 1, 1)) rospy.sleep(3) # Waiting for setting conveyer #stand rot_o = 0.0 / 180.0 * math.pi rot_a = 0.0 / 180.0 * math.pi rot_t = 0.0 / 180.0 * math.pi q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx') #setting origin of stand box_pose.pose.position.x = -0.385 box_pose.pose.position.y = 0.2342 box_pose.pose.position.z = 0.0 box_pose.pose.orientation.x = q[0] box_pose.pose.orientation.y = q[1] box_pose.pose.orientation.z = q[2] box_pose.pose.orientation.w = q[3] meshpath = resourcepath + "stand.stl" scene.add_mesh('stand', box_pose, meshpath, (1, 1, 1)) rospy.sleep(3) # Waiting for setting stand rospy.loginfo("Planning Scene Settings Finish") #tray rot_o = 0.0 / 180.0 * math.pi rot_a = 0.0 / 180.0 * math.pi rot_t = 0.0 / 180.0 * math.pi q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx') #setting origin of tray box_pose.pose.position.x = -0.58952 box_pose.pose.position.y = -0.0613 box_pose.pose.position.z = 0.6375 box_pose.pose.orientation.x = q[0] box_pose.pose.orientation.y = q[1] box_pose.pose.orientation.z = q[2] box_pose.pose.orientation.w = q[3] meshpath = resourcepath + "tray_nooffset.stl" scene.add_mesh('tray', box_pose, meshpath, (1, 1, 1)) rospy.loginfo("Planning Scene Settings Finish")
class Move(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.br = tf.TransformBroadcaster() self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) def initial_code(self): planning_frame = self.robot_arm.get_planning_frame() print("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print("====== current pose : ", self.wpose) joint_goal = [ 2.3884003162384033, -1.6826804319964808, 0.7239289283752441, -2.2834256331073206, 4.623193740844727, -0.025881592427388966 ] print("init pose : ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait=True) def go_to_move(self): planning_frame = self.robot_arm.get_planning_frame() print("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print("====== current pose : ", self.wpose) joint_goal = [ 2.3886876106262207, -1.6729376951800745, 1.6410179138183594, -2.855138603840963, 4.623205661773682, -0.025869671498433888 ] print("move_pose : ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait=True) def go_to_tomato(self): planning_frame = self.robot_arm.get_planning_frame() print("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print("====== current pose : ", self.wpose) joint_goal = [ 2.238821506500244, -0.8660662809955042, 0.8210840225219727, -2.8785727659808558, 4.622917175292969, -0.025917832051412404 ] print("tomato_pose : ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait=True) def plan_cartesian_y(self, y_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True)
class Arm(object): def __init__(self, move_group): # create move group self.group = MoveGroupCommander(move_group) def get_pose(self): """ Get current pose for specified move group Returns ros msg (geometry_msgs.msg._PoseStamped.PoseStamped) - A message specifying the pose of the move group as position (XYZ) and orientation (xyzw). """ return self.group.get_current_pose().pose def get_joint_values(self): """ Get joint values for specified move group Returns joint value (list) - A list of joint values in radians """ return self.group.get_current_joint_values() def get_planning_frame(self): """ Get planning frame of move group """ return self.group.get_planning_frame() def clear_pose_targets(self, move_group): """ Clear target pose for planning """ self.group.clear_pose_targets() def plan_to_pose(self, pose): """ Plan to a given pose for the end-effector Args pose (msg): geometry_msgs.msg.Pose """ # Clear old pose targets self.group.clear_pose_targets() # Set target pose self.group.set_pose_target(pose) numTries = 0 while numTries < 5: plan = self.group.plan() numTries += 1 if len(plan.joint_trajectory.points) > 0: print('succeeded in %d tries' % numTries) return True print('Planning failed') return False def plan_to_joints(self, joint_angles): """Plan to a given joint configuration Args joint_angles (list of floats): list of len 7 of joint angles in radians Returns True if planning succeeds, False if not """ # Clear old pose targets self.group.clear_pose_targets() # Set Joint configuration target self.group.set_joint_value_target(joint_angles) numTries = 0 while numTries < 5: plan = group.plan() numTries += 1 if len(plan.joint_trajectory.points) > 0: print('succeeded in %d tries' % numTries) return True print("Planning failed") return False def move_to_joints(self, joint_angles): """ Move to specified joint configuration Args joint_angles (list of floats): list of len 7 of joint angles in radians blocking (bool): If True, Returns True if planning succeeds, False if not """ # plan plan = self.plan_to_joints(group.get_name(), joint_angles) # move to joint configuration if plan: group.go(wait=True) return plan def move_to_pose(self, pose): """ Move to specified joint configuration Args pose (msg): geometry_msgs.msg.Pose joint_angles (list of floats): list of len 7 of joint angles in radians blocking (bool): If True, Returns True if planning succeeds, False if not """ # plan plan = self.plan_to_pose(pose) if plan: group.go(wait=blocking) group.stop() group.clear_pose_targets() return def _change_planner(self, planner): planners = [ "SBLkConfigDefault", "ESTkConfigDefault", "LBKPIECEkConfigDefault", "BKPIECEkConfigDefault", "KPIECEkConfigDefault", "RRTkConfigDefault", "RRTConnectkConfigDefault", "RRTstarkConfigDefault", "TRRTkConfigDefault", "PRMkConfigDefault", "PRMstarkConfigDefault" ] if planner in planners: self.group.set_planner_id(planner) return def create_subscriber(self, topic, message_type): return
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback) rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.ii = 0 global goal_x global goal_y global goal_z # self.listener = tf.TransformListener() #self.goal_x = 0 #self.goal_y = 0 #self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 # self.marker = [] self.tomato = [] self.position_list = [] self.orientation_list = [] self.t_idd = 0 self.t_pose_x = [] self.t_pose_y = [] self.t_pose_z = [] self.t_ori_w = [] self.t_ori_x = [] self.t_ori_y = [] self.t_ori_z = [] self.tomato_pose = Pose() self.goalPoseFromTomato = Pose() self.br = tf.TransformBroadcaster() self.calculated_tomato = Pose() self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.display_trajectory_publisher = display_trajectory_publisher rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print("====== planning frame: ", planning_frame) self.wpose = self.robot_arm.get_current_pose() print("====== current pose : ", self.wpose) joint_goal = [ -0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982 ] print("INIT POSE: ", self.robot_arm.get_current_pose().pose.position) self.robot_arm.go(joint_goal, wait=True) def go_to_move(self, scale=1.0): planning_frame = self.robot_arm.get_planning_frame() tomato_offset = [0, -0.35, -0.1] robot_base_offset = 0.873 base_wrist2_offset = 0.1 print(">> robot arm planning frame: \n", planning_frame) ''' self.calculated_tomato.position.x = (scale*self.goal_x) self.calculated_tomato.position.y = (scale*self.goal_y) self.calculated_tomato.position.z = (scale*self.goal_z) ''' self.calculated_tomato.position.x = (scale * goal_x) self.calculated_tomato.position.y = (scale * goal_y) self.calculated_tomato.position.z = (scale * goal_z) self.calculated_tomato.orientation = Quaternion( *quaternion_from_euler(3.14, 0.1, 1.57)) print("=== robot_pose goal frame: ", self.calculated_tomato) self.robot_arm.set_pose_target(self.calculated_tomato) tf_display_position = [ self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z ] tf_display_orientation = [ self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() tomato_waypoints = [] tomato_waypoints.append(copy.deepcopy(self.calculated_tomato)) (tomato_plan, tomato_offset) = self.robot_arm.compute_cartesian_path( tomato_waypoints, 0.01, 0.0) self.display_trajectory(tomato_plan) print("=== Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch, yaw): ''' Manipulator is moving to the desired coordinate Now move to the calculated_tomato_id_goal ''' calculated_tomato_id_goal = Pose() desired_goal_pose = [pose_x, pose_y, pose_z] desired_goal_euler = [roll, pitch, yaw] Cplanning_frame = self.robot_arm.get_planning_frame() print(">> current planning frame: \n", Cplanning_frame) calculated_tomato_id_goal.position.x = desired_goal_pose[0] calculated_tomato_id_goal.position.y = desired_goal_pose[1] calculated_tomato_id_goal.position.z = desired_goal_pose[2] calculated_tomato_id_goal.orientation = Quaternion( *quaternion_from_euler(desired_goal_euler[0], desired_goal_euler[1], desired_goal_euler[2])) print(">> tomato_id_goal frame: ", desired_goal_pose) self.robot_arm.set_pose_target(calculated_tomato_id_goal) tf_display_position = [ calculated_tomato_id_goal.position.x, calculated_tomato_id_goal.position.x, calculated_tomato_id_goal.position.z ] tf_display_orientation = [ calculated_tomato_id_goal.orientation.x, calculated_tomato_id_goal.orientation.y, calculated_tomato_id_goal.orientation.z, calculated_tomato_id_goal.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() ## ## ## show how to move on the Rviz tomato_id_goal_waypoints = [] tomato_id_goal_waypoints.append( copy.deepcopy(calculated_tomato_id_goal)) (tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path( tomato_id_goal_waypoints, 0.01, 0.0) self.display_trajectory(tomato_id_goal_plan) print("============ Press `Enter` to if plan is correct!! ...") raw_input() self.robot_arm.go(True) def display_trajectory(self, plan): display_trajectory_publisher = self.display_trajectory_publisher display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = self.robot_cmd.get_current_state( ) display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.z = (scale * self.wpose.position.z) + z_offset waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_x(self, x_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_y(self, y_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def plan_cartesian_z(self, z_offset, scale=1.0): waypoints = [] self.wpose = self.robot_arm.get_current_pose().pose self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10 waypoints.append(copy.deepcopy(self.wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def pose_subscriber(self): rospy.loginfo("waiting for pose topic...") rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback) rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") #self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.marker = [] self.position_list = [] self.orientation_list = [] self.m_idd = 0 self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.ar_pose = Pose() self.goalPoseFromAR = Pose() self.br = tf.TransformBroadcaster() #self.goalPose_from_arPose = Pose() self.trans = [] self.rot = [] self.calculed_coke_pose = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print "====== current pose : ", self.wpose marker_joint_goal = [ 0.07100913858593216, -1.8767615298285376, 2.0393206555899503, -1.8313959190971882, -0.6278395875738125, 1.6918219826764682 ] self.robot_arm.go(marker_joint_goal, wait=True) def look_object(self): look_object = self.robot_arm.get_current_joint_values() ## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함 print "wrist3 joint value(deg, rad): ", math.degrees( look_object[5]), look_object[5] look_object[5] = math.radians(90) ''' if look_object[5] > abs(math.radians(180)): if look_object[5] < 0: look_object[5] = look_object[5] + math.radians(90) # wrist3 elif look_object[5] > 0: look_object[5] = look_object[5] - math.radians(90) else: look_object[5] = look_object[5] - math.radians(90) # wrist3 ''' print "wrist3 joint value(deg, rad): ", math.degrees( look_object[5]), look_object[5] #look_object[3] = look_object[3] - (math.radians(00)) # wrist1 self.robot_arm.go(look_object, wait=True) #look_object[5] = look_object[5] + (math.radians(90)) # wrist3 #self.robot_arm.go(look_object, wait=True) def look_up_down(self): self.clear_octomap() print "======== clear_octomap... Please wait...." look_up_down = self.robot_arm.get_current_joint_values() #print "before: ", look_up_down look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) def plan_cartesian_path(self, x_offset, y_offset, scale=1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose print "===== robot arm pose: ", self.wpose self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10 #print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 self.wpose.position.y = (scale * self.goal_y) + y_offset # + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def print_ar_pose(self): rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener) rospy.loginfo("Maker messages detected. Starting followers...") print "======= pos(meter): ", self.position_list print "======= orientation: ", self.orientation_list def go_to_move(self, scale=1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기 #self.calculed_coke_pose = self.robot_arm.get_current_pose() planning_frame = self.robot_arm.get_planning_frame() print "========== robot arm plannig frame: \n", planning_frame self.calculed_coke_pose.position.x = ( scale * self.goal_x) + 0.10 # base_link to wrist2 x-offset self.calculed_coke_pose.position.y = (scale * self.goal_y) - 0.25 #self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 - 0.115 # world to base_link z-offset and coke can offset self.calculed_coke_pose.position.z = ( scale * self.goal_z ) + 0.72 + 0.2 # world to base_link z-offset real version offset self.calculed_coke_pose.orientation = Quaternion( *quaternion_from_euler(0, 0, 1.54)) print "========== coke_pose goal frame: ", self.calculed_coke_pose self.robot_arm.set_pose_target(self.calculed_coke_pose) tf_display_position = [ self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z ] tf_display_orientation = [ self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w ] ii = 0 while ii < 5: ii += 1 self.br.sendTransform(tf_display_position, tf_display_orientation, rospy.Time.now(), "goal_wpose", "world") rate.sleep() print "============ Press `Enter` to if plan is correct!! ..." raw_input() self.robot_arm.go(True) def ar_tf_listener(self, msg): try: self.marker = msg.markers ml = len(self.marker) target_id = 9 #self.m_idd = self.marker[0].id # 임시용 for ii in range(0, ml): # 0 <= ii < ml self.m_idd = self.marker[ii].id #print "checked all id: ", self.m_idd if self.m_idd != target_id: pass #target_id_flage = False elif self.m_idd == target_id: target_id_flage = True target_id = self.m_idd target_id_index = ii #print "target id: ", target_id_index, target_id, target_id_flage if target_id_flage == True: self.ar_pose.position.x = self.marker[ target_id_index].pose.pose.position.x self.ar_pose.position.y = self.marker[ target_id_index].pose.pose.position.y self.ar_pose.position.z = self.marker[ target_id_index].pose.pose.position.z self.ar_pose.orientation.x = self.marker[ target_id_index].pose.pose.orientation.x self.ar_pose.orientation.y = self.marker[ target_id_index].pose.pose.orientation.y self.ar_pose.orientation.z = self.marker[ target_id_index].pose.pose.orientation.z self.ar_pose.orientation.w = self.marker[ target_id_index].pose.pose.orientation.w self.goal_x = self.ar_pose.position.x self.goal_y = self.ar_pose.position.y self.goal_z = self.ar_pose.position.z self.position_list = [self.goal_x, self.goal_y, self.goal_z] self.orientation_list = [ self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w ] (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion( self.orientation_list) #list form으로 넘겨주어야 함 #print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z #print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw #print "ar_pos(meter): \n", self.position_list #print "ar_orientation: \n", self.orientation_list except: return
class SrRobotCommander(object): """ Base class for hand and arm commanders """ def __init__(self, name): """ Initialize MoveGroupCommander object @param name - name of the MoveIt group """ self._name = name self._move_group_commander = MoveGroupCommander(name) self._robot_commander = RobotCommander() self._robot_name = self._robot_commander._r.get_robot_name() self.refresh_named_targets() self._warehouse_name_get_srv = rospy.ServiceProxy( "get_robot_state", GetState) self._planning_scene = PlanningSceneInterface() self._joint_states_lock = threading.Lock() self._joint_states_listener = \ rospy.Subscriber("joint_states", JointState, self._joint_states_callback, queue_size=1) self._joints_position = {} self._joints_velocity = {} self._joints_effort = {} self._joints_state = None self._clients = {} self.__plan = None self._controllers = {} rospy.wait_for_service('compute_ik') self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) controller_list_param = rospy.get_param("/move_group/controller_list") # create dictionary with name of controllers and corresponding joints self._controllers = { item["name"]: item["joints"] for item in controller_list_param } self._set_up_action_client(self._controllers) self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) threading.Thread(None, rospy.spin) def set_planner_id(self, planner_id): self._move_group_commander.set_planner_id(planner_id) def set_num_planning_attempts(self, num_planning_attempts): self._move_group_commander.set_num_planning_attempts( num_planning_attempts) def set_planning_time(self, seconds): self._move_group_commander.set_planning_time(seconds) def get_end_effector_pose_from_named_state(self, name): state = self._warehouse_name_get_srv(name, self._robot_name).state return self.get_end_effector_pose_from_state(state) def get_end_effector_pose_from_state(self, state): header = Header() fk_link_names = [self._move_group_commander.get_end_effector_link()] header.frame_id = self._move_group_commander.get_pose_reference_frame() response = self._forward_k(header, fk_link_names, state) return response.pose_stamped[0] def get_planning_frame(self): return self._move_group_commander.get_planning_frame() def set_pose_reference_frame(self, reference_frame): self._move_group_commander.set_pose_reference_frame(reference_frame) def get_group_name(self): return self._name def refresh_named_targets(self): self._srdf_names = self.__get_srdf_names() self._warehouse_names = self.__get_warehouse_names() def set_max_velocity_scaling_factor(self, value): self._move_group_commander.set_max_velocity_scaling_factor(value) def set_max_acceleration_scaling_factor(self, value): self._move_group_commander.set_max_acceleration_scaling_factor(value) def allow_looking(self, value): self._move_group_commander.allow_looking(value) def allow_replanning(self, value): self._move_group_commander.allow_replanning(value) def execute(self): """ Executes the last plan made. """ if self.check_plan_is_valid(): self._move_group_commander.execute(self.__plan) self.__plan = None else: rospy.logwarn("No plans were made, not executing anything.") def execute_plan(self, plan): if self.check_given_plan_is_valid(plan): self._move_group_commander.execute(plan) self.__plan = None else: rospy.logwarn("Plan is not valid, not executing anything.") def move_to_joint_value_target(self, joint_states, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self._move_group_commander.go(wait=wait) def plan_to_joint_value_target(self, joint_states, angle_degrees=False): """ Set target of the robot's links and plans. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param angle_degrees - are joint_states in degrees or not This is a blocking method. """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self.__plan = self._move_group_commander.plan() return self.__plan def check_plan_is_valid(self): """ Checks if current plan contains a valid trajectory """ return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0) def check_given_plan_is_valid(self, plan): """ Checks if given plan contains a valid trajectory """ return (plan is not None and len(plan.joint_trajectory.points) > 0) def get_robot_name(self): return self._robot_name def named_target_in_srdf(self, name): return name in self._srdf_names def set_named_target(self, name): if name in self._srdf_names: self._move_group_commander.set_named_target(name) elif (name in self._warehouse_names): response = self._warehouse_name_get_srv(name, self._robot_name) active_names = self._move_group_commander._g.get_active_joints() joints = response.state.joint_state.name positions = response.state.joint_state.position js = {} for n, this_name in enumerate(joints): if this_name in active_names: js[this_name] = positions[n] self._move_group_commander.set_joint_value_target(js) else: rospy.logerr("Unknown named state '%s'..." % name) return False return True def get_named_target_joint_values(self, name): output = dict() if (name in self._srdf_names): output = self._move_group_commander.\ _g.get_named_target_values(str(name)) elif (name in self._warehouse_names): js = self._warehouse_name_get_srv( name, self._robot_name).state.joint_state for x, n in enumerate(js.name): if n in self._move_group_commander._g.get_joints(): output[n] = js.position[x] else: rospy.logerr("No target named %s" % name) return None return output def get_end_effector_link(self): return self._move_group_commander.get_end_effector_link() def get_current_pose(self, reference_frame=None): """ Get the current pose of the end effector. @param reference_frame - The desired reference frame in which end effector pose should be returned. If none is passed, it will use the planning frame as reference. @return geometry_msgs.msg.Pose() - current pose of the end effector """ if reference_frame is not None: try: trans = self.tf_buffer.lookup_transform( reference_frame, self._move_group_commander.get_end_effector_link(), rospy.Time(0), rospy.Duration(5.0)) current_pose = geometry_msgs.msg.Pose() current_pose.position.x = trans.transform.translation.x current_pose.position.y = trans.transform.translation.y current_pose.position.z = trans.transform.translation.z current_pose.orientation.x = trans.transform.rotation.x current_pose.orientation.y = trans.transform.rotation.y current_pose.orientation.z = trans.transform.rotation.z current_pose.orientation.w = trans.transform.rotation.w return current_pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logwarn( "Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() + " in " + reference_frame + " reference frame") return None else: return self._move_group_commander.get_current_pose().pose def get_current_state(self): """ Get the current joint state of the group being used. @return a dictionary with the joint names as keys and current joint values """ joint_names = self._move_group_commander._g.get_active_joints() joint_values = self._move_group_commander._g.get_current_joint_values() return dict(zip(joint_names, joint_values)) def get_current_state_bounded(self): """ Get the current joint state of the group being used, enforcing that they are within each joint limits. @return a dictionary with the joint names as keys and current joint values """ current = self._move_group_commander._g.get_current_state_bounded() names = self._move_group_commander._g.get_active_joints() output = {n: current[n] for n in names if n in current} return output def get_robot_state_bounded(self): return self._move_group_commander._g.get_current_state_bounded() def move_to_named_target(self, name, wait=True): """ Set target of the robot's links and moves to it @param name - name of the target pose defined in SRDF @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self._move_group_commander.go(wait=wait) def plan_to_named_target(self, name): """ Set target of the robot's links and plans This is a blocking method. @param name - name of the target pose defined in SRDF """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self.__plan = self._move_group_commander.plan() def __get_warehouse_names(self): try: list_srv = rospy.ServiceProxy("list_robot_states", ListStates) return list_srv("", self._robot_name).states except rospy.ServiceException as exc: rospy.logwarn("Couldn't access warehouse: " + str(exc)) return list() def _reset_plan(self): self.__plan = None def _set_plan(self, plan): self.__plan = plan def __get_srdf_names(self): return self._move_group_commander._g.get_named_targets() def get_named_targets(self): """ Get the complete list of named targets, from SRDF as well as warehouse poses if available. @return list of strings containing names of targets. """ return self._srdf_names + self._warehouse_names def get_joints_position(self): """ Returns joints position @return - dictionary with joints positions """ with self._joint_states_lock: return self._joints_position def get_joints_velocity(self): """ Returns joints velocities @return - dictionary with joints velocities """ with self._joint_states_lock: return self._joints_velocity def _get_joints_effort(self): """ Returns joints effort @return - dictionary with joints efforts """ with self._joint_states_lock: return self._joints_effort def get_joints_state(self): """ Returns joints state @return - JointState message """ with self._joint_states_lock: return self._joints_state def run_joint_trajectory(self, joint_trajectory): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. """ plan = RobotTrajectory() plan.joint_trajectory = joint_trajectory self._move_group_commander.execute(plan) def make_named_trajectory(self, trajectory): """ Makes joint value trajectory from specified by named poses (either from SRDF or from warehouse) @param trajectory - list of waypoints, each waypoint is a dict with the following elements (n.b either name or joint_angles is required) - name -> the name of the way point - joint_angles -> a dict of joint names and angles - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent. """ current = self.get_current_state_bounded() joint_trajectory = JointTrajectory() joint_names = current.keys() joint_trajectory.joint_names = joint_names start = JointTrajectoryPoint() start.positions = current.values() start.time_from_start = rospy.Duration.from_sec(0.001) joint_trajectory.points.append(start) time_from_start = 0.0 for wp in trajectory: joint_positions = None if 'name' in wp.keys(): joint_positions = self.get_named_target_joint_values( wp['name']) elif 'joint_angles' in wp.keys(): joint_positions = copy.deepcopy(wp['joint_angles']) if 'degrees' in wp.keys() and wp['degrees']: for joint, angle in joint_positions.iteritems(): joint_positions[joint] = radians(angle) if joint_positions is None: rospy.logerr( "Invalid waypoint. Must contain valid name for named target or dict of joint angles." ) return None new_positions = {} for n in joint_names: new_positions[n] = joint_positions[ n] if n in joint_positions else current[n] trajectory_point = JointTrajectoryPoint() trajectory_point.positions = [ new_positions[n] for n in joint_names ] current = new_positions time_from_start += wp['interpolate_time'] trajectory_point.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(trajectory_point) if 'pause_time' in wp and wp['pause_time'] > 0: extra = JointTrajectoryPoint() extra.positions = trajectory_point.positions time_from_start += wp['pause_time'] extra.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(extra) return joint_trajectory def send_stop_trajectory_unsafe(self): """ Sends a trajectory of all active joints at their current position. This stops the robot. """ current = self.get_current_state_bounded() trajectory_point = JointTrajectoryPoint() trajectory_point.positions = current.values() trajectory_point.time_from_start = rospy.Duration.from_sec(0.1) trajectory = JointTrajectory() trajectory.points.append(trajectory_point) trajectory.joint_names = current.keys() self.run_joint_trajectory_unsafe(trajectory) def run_named_trajectory_unsafe(self, trajectory, wait=False): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory directly via contoller. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory_unsafe(joint_trajectory, wait) def run_named_trajectory(self, trajectory): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory via moveit. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory(joint_trajectory) def move_to_position_target(self, xyz, end_effector_link="", wait=True): """ Specify a target position for the end-effector and moves to it @param xyz - new position of end-effector @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_position_target(self, xyz, end_effector_link=""): """ Specify a target position for the end-effector and plans. This is a blocking method. @param xyz - new position of end-effector @param end_effector_link - name of the end effector link """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self.__plan = self._move_group_commander.plan() def move_to_pose_target(self, pose, end_effector_link="", wait=True): """ Specify a target pose for the end-effector and moves to it @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_pose_target(pose, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_pose_target(self, pose, end_effector_link="", alternative_method=False): """ Specify a target pose for the end-effector and plans. This is a blocking method. @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param alternative_method - use set_joint_value_target instead of set_pose_target """ self._move_group_commander.set_start_state_to_current_state() if alternative_method: self._move_group_commander.set_joint_value_target( pose, end_effector_link) else: self._move_group_commander.set_pose_target(pose, end_effector_link) self.__plan = self._move_group_commander.plan() return self.__plan def _joint_states_callback(self, joint_state): """ The callback function for the topic joint_states. It will store the received joint position, velocity and efforts information into dictionaries @param joint_state - the message containing the joints data. """ with self._joint_states_lock: self._joints_state = joint_state self._joints_position = { n: p for n, p in zip(joint_state.name, joint_state.position) } self._joints_velocity = { n: v for n, v in zip(joint_state.name, joint_state.velocity) } self._joints_effort = { n: v for n, v in zip(joint_state.name, joint_state.effort) } def _set_up_action_client(self, controller_list): """ Sets up an action client to communicate with the trajectory controller """ self._action_running = {} for controller_name in controller_list.keys(): self._action_running[controller_name] = False service_name = controller_name + "/follow_joint_trajectory" self._clients[controller_name] = SimpleActionClient( service_name, FollowJointTrajectoryAction) if self._clients[controller_name].wait_for_server( timeout=rospy.Duration(4)) is False: err_msg = 'Failed to connect to action server ({}) in 4 sec'.format( service_name) rospy.logwarn(err_msg) def move_to_joint_value_target_unsafe(self, joint_states, time=0.002, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param time - time in s (counting from now) for the robot to reach the target (it needs to be greater than 0.0 for it not to be rejected by the trajectory controller) @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ # self._update_default_trajectory() # self._set_targets_to_default_trajectory(joint_states) goals = {} joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [] point = JointTrajectoryPoint() point.positions = [] for x in joint_states_cpy.keys(): if x in controller_joints: goal.trajectory.joint_names.append(x) point.positions.append(joint_states_cpy[x]) point.time_from_start = rospy.Duration.from_sec(time) goal.trajectory.points = [point] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def action_is_running(self, controller=None): if controller is not None: return self._action_running[controller] for controller_running in self._action_running.values(): if controller_running: return True return False def _action_done_cb(self, controller, terminal_state, result): self._action_running[controller] = False def _call_action(self, goals): for client in self._clients: self._action_running[client] = True self._clients[client].send_goal( goals[client], lambda terminal_state, result: self._action_done_cb( client, terminal_state, result)) def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. @param wait - should method wait for movement end or not """ goals = {} for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory = copy.deepcopy(joint_trajectory) indices_of_joints_in_this_controller = [] for i, joint in enumerate(joint_trajectory.joint_names): if joint in controller_joints: indices_of_joints_in_this_controller.append(i) goal.trajectory.joint_names = [ joint_trajectory.joint_names[i] for i in indices_of_joints_in_this_controller ] for point in goal.trajectory.points: if point.positions: point.positions = [ point.positions[i] for i in indices_of_joints_in_this_controller ] if point.velocities: point.velocities = [ point.velocities[i] for i in indices_of_joints_in_this_controller ] if point.effort: point.effort = [ point.effort[i] for i in indices_of_joints_in_this_controller ] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def plan_to_waypoints_target(self, waypoints, reference_frame=None, eef_step=0.005, jump_threshold=0.0): """ Specify a set of waypoints for the end-effector and plans. This is a blocking method. @param reference_frame - the reference frame in which the waypoints are given @param waypoints - an array of poses of end-effector @param eef_step - configurations are computed for every eef_step meters @param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path """ old_frame = self._move_group_commander.get_pose_reference_frame() if reference_frame is not None: self.set_pose_reference_frame(reference_frame) (self.__plan, fraction) = self._move_group_commander.compute_cartesian_path( waypoints, eef_step, jump_threshold) self.set_pose_reference_frame(old_frame) def set_teach_mode(self, teach): """ Activates/deactivates the teach mode for the robot. Activation: stops the the trajectory controllers for the robot, and sets it to teach mode. Deactivation: stops the teach mode and starts trajectory controllers for the robot. Currently this method blocks for a few seconds when called on a hand, while the hand parameters are reloaded. @param teach - bool to activate or deactivate teach mode """ if teach: mode = RobotTeachModeRequest.TEACH_MODE else: mode = RobotTeachModeRequest.TRAJECTORY_MODE self.change_teach_mode(mode, self._name) def move_to_trajectory_start(self, trajectory, wait=True): """ Make and execute a plan from the current state to the first state in an pre-existing trajectory @param trajectory - moveit_msgs/JointTrajectory @param wait - Bool to specify if movement should block untill finished. """ if len(trajectory.points) <= 0: rospy.logerr("Trajectory has no points in it, can't reverse...") return None first_point = trajectory.points[0] end_state = dict(zip(trajectory.joint_names, first_point.positions)) self.move_to_joint_value_target(end_state, wait=wait) @staticmethod def change_teach_mode(mode, robot): teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode) req = RobotTeachModeRequest() req.teach_mode = mode req.robot = robot try: resp = teach_mode_client(req) if resp.result == RobotTeachModeResponse.ERROR: rospy.logerr("Failed to change robot %s to mode %d", robot, mode) else: rospy.loginfo("Changed robot %s to mode %d Result = %d", robot, mode, resp.result) except rospy.ServiceException: rospy.logerr("Failed to call service teach_mode") def get_ik(self, target_pose, avoid_collisions=False, joint_states=None): """ Computes the inverse kinematics for a given pose. It returns a JointState @param target_pose - A given pose of type PoseStamped @param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false """ service_request = PositionIKRequest() service_request.group_name = self._name service_request.ik_link_name = self._move_group_commander.get_end_effector_link( ) service_request.pose_stamped = target_pose service_request.timeout.secs = 0.5 service_request.avoid_collisions = avoid_collisions if joint_states is None: service_request.robot_state.joint_state = self.get_joints_state() else: service_request.robot_state.joint_state = joint_states try: resp = self._compute_ik(ik_request=service_request) # Check if error_code.val is SUCCESS=1 if resp.error_code.val != 1: if resp.error_code.val == -10: rospy.logerr("Unreachable point: Start state in collision") elif resp.error_code.val == -12: rospy.logerr("Unreachable point: Goal state in collision") elif resp.error_code.val == -31: rospy.logerr("Unreachable point: No IK solution") else: rospy.logerr("Unreachable point (error: %s)" % resp.error_code) return else: return resp.solution.joint_state except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) #self.clear_octomap() def ar_callback(self, msg): #marker = msg.markers[1] self.marker = msg.markers ml = len(self.marker) m_id = self.marker[0].id print "marker length: ", len(self.marker) print "marker id: ", m_id m_idd = [] ''' for ii in range(0, 2): print(ii) m_idd[ii] = marker[ii].id #m_id[ii] = marker[ii].id print(m_idd[ii]) ''' pos_x = self.marker[0].pose.pose.position.x pos_y = self.marker[0].pose.pose.position.y pos_z = self.marker[0].pose.pose.position.z dist = math.sqrt((pos_x * pos_x) + (pos_y * pos_y)) #ori_x = marker.pose.pose.orientation.x #ori_y = marker.pose.pose.orientation.y #ori_z = marker.pose.pose.orientation.z #ori_w = marker.pose.pose.orientation.w #print(m_id) print "===========" print 'id: ', m_id print 'pos: ', pos_x, pos_y, pos_z #print('ori: ', ori_x, ori_y, ori_z, ori_w) self.goal_x = pos_x - 1.0 self.goal_y = pos_y - pos_y self.goal_z = pos_z - pos_z def move_code(self): robot_state = self.robot_arm.get_current_pose() print "====== robot pose: \n", print robot_state.pose.position #marker_joint_goal = self.robot_arm.get_current_joint_values() marker_joint_goal = [ 0.07100913858593216, -1.8767615298285376, 2.0393206555899503, -1.8313959190971882, -0.6278395875738125, 1.6918219826764682 ] self.robot_arm.go(marker_joint_goal, wait=True) print "====== robot joint value: \n" print marker_joint_goal self.robot_arm.go(marker_joint_goal, wait=True) self.robot_arm.go(True) print "look at the markers" pose_goal = Pose() pose_goal.orientation.w = 0.0 pose_goal.position.x = 0.4 pose_goal.position.y = -0.4 pose_goal.position.z = 0.7 planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame #self.robot_arm.set_pose_target(pose_goal) #7self.robot_arm.go(True) def plan_cartesian_path(self, scale=1): waypoints = [] wpose = self.robot_arm.get_current_pose().pose wpose.position.z -= scale * 0.2 wpose.position.x -= scale * 0.2 waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.robot_arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def print_ar_pose(self): rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) rospy.loginfo("Maker messages detected. Starting followers...")
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm left_arm = MoveGroupCommander('left_arm') left_gripper = MoveGroupCommander('left_gripper') # Get the name of the end-effector link left_eef = left_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) left_arm.set_goal_position_tolerance(0.01) left_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution left_arm.allow_replanning(True) left_reference_frame = left_arm.get_planning_frame() # Allow 5 seconds per planning attempt left_arm.set_planning_time(5) object1_id = 'object1' obstacle1_id = 'obstacle1' # Remove leftover objects from a previous run scene.remove_world_object(object1_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file left_arm.set_named_target('left_arm_zero') left_arm.go() rospy.sleep(1) left_gripper.set_joint_value_target(GRIPPER_OPEN) left_gripper.go() rospy.sleep(1) object1_size = [0.088, 0.04, 0.02] # Add a table top and two boxes to the scene obstacle1_size = [0.3, 0.05, 0.45] # Add a table top and two boxes to the scene obstacle1_pose = PoseStamped() obstacle1_pose.header.frame_id = left_reference_frame obstacle1_pose.pose.position.x = 0.96 obstacle1_pose.pose.position.y = 0.5 obstacle1_pose.pose.position.z = 0.04 obstacle1_pose.pose.orientation.w = 1.0 scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size) self.setColor(obstacle1_id, 0.8, 0.4, 0, 1.0) object1_pose = PoseStamped() object1_pose.header.frame_id = left_reference_frame object1_pose.pose.position.x = 0.80 object1_pose.pose.position.y = 0.3 object1_pose.pose.position.z = 0 object1_pose.pose.orientation.w = 1.0 scene.add_box(object1_id, object1_pose, object1_size) self.setColor(object1_id, 0.8, 0, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the start state to the current state left_arm.set_start_state_to_current_state() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = left_reference_frame target_pose.pose.position.x = 0.58 target_pose.pose.position.y = 0.1878 target_pose.pose.position.z = .1116 target_pose.pose.orientation.x = 0.1325 target_pose.pose.orientation.y = 0.9908 target_pose.pose.orientation.z = 0.0095 target_pose.pose.orientation.w = 0.0254 # Set the target pose for the arm left_arm.set_pose_target(target_pose, left_eef) # Move the arm to the target pose (if possible) left_arm.go() # Pause for a moment... rospy.sleep(2) left_gripper.set_joint_value_target(GRIPPER_CLOSE) left_gripper.go() rospy.sleep(1) scene.attach_box(left_eef, object1_id, object1_pose, object1_size) # Cartesian Paths waypoints1 = [] start_pose = left_arm.get_current_pose(left_eef).pose wpose = deepcopy(start_pose) waypoints1.append(deepcopy(wpose)) wpose.position.x -= 0.05 wpose.position.y += 0.105 wpose.position.z += 0.07 waypoints1.append(deepcopy(wpose)) wpose.position.x -= 0.05 wpose.position.y += 0.105 wpose.position.z -= 0.07 waypoints1.append(deepcopy(wpose)) (cartesian_plan, fraction) = left_arm.compute_cartesian_path( waypoints1, # waypoint poses 0.01, # eef_step 1cm 0.0, # jump_threshold True) # avoid_collisions left_arm.execute(cartesian_plan) rospy.sleep(2) left_gripper.set_joint_value_target(GRIPPER_OPEN) left_gripper.go() rospy.sleep(1) scene.remove_attached_object(left_eef, object1_id) # Exit MoveIt cleanly waypoints2 = [] start_pose = left_arm.get_current_pose(left_eef).pose wpose = deepcopy(start_pose) waypoints2.append(deepcopy(wpose)) wpose.position.z += 0.07 waypoints2.append(deepcopy(wpose)) wpose.position.x += 0.1 wpose.position.y -= 0.21 waypoints2.append(deepcopy(wpose)) wpose.position.z -= 0.07 waypoints2.append(deepcopy(wpose)) (cartesian_plan, fraction) = left_arm.compute_cartesian_path( waypoints2, # waypoint poses 0.01, # eef_step 1cm 0.0, # jump_threshold True) # avoid_collisions left_arm.execute(cartesian_plan) rospy.sleep(2) moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move',anonymous=True) self.listener = tf.TransformListener() self.goal_x = 0 self.goal_y = 0 self.goal_z = 0 self.goal_roll = 0 self.goal_pitch = 0 self.goal_yaw = 0 self.goal_ori_x = 0 self.goal_ori_y = 0 self.goal_ori_z = 0 self.goal_ori_w = 0 self.wpose = [] self.marker = [] self.tf_list = [] self.m_idd = [] self.m_pose_x = [] self.m_pose_y = [] self.m_pose_z = [] self.m_ori_w = [] self.m_ori_x = [] self.m_ori_y = [] self.m_ori_z = [] self.trans = [] self.rot = [] self.pose_goal = Pose() #rospy.loginfo("Waiting for ar_pose_marker topic...") #rospy.wait_for_message('ar_pose_marker', AlvarMarkers) #rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) #rospy.loginfo("Maker messages detected. Starting followers...") self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) self.clear_octomap() def ar_callback(self, msg): #marker = msg.markers[1] self.marker = msg.markers ml = len(self.marker) m_id = self.marker[0].id #print "marker length: ", ml, ml-1 #print "marker id: ", m_id, self.marker[1].id #self.m_idd = [] #self.m_pose_x = [] #self.m_pose_y = [] #self.m_pose_z = [] #self.m_ori_w = [] #self.m_ori_x = [] #self.m_ori_y = [] #self.m_ori_z = [] for ii in range(0, ml): self.m_idd.append(self.marker[ii].id) self.m_pose_x.append(self.marker[ii].pose.pose.position.x) self.m_pose_y.append(self.marker[ii].pose.pose.position.y) self.m_pose_z.append(self.marker[ii].pose.pose.position.z) self.m_ori_w.append(self.marker[ii].pose.pose.orientation.w) self.m_ori_x.append(self.marker[ii].pose.pose.orientation.x) self.m_ori_y.append(self.marker[ii].pose.pose.orientation.y) self.m_ori_z.append(self.marker[ii].pose.pose.orientation.z) ''' print "id: ", self.m_idd print "pose_x: ", self.m_pose_x print "pose_y: ", self.m_pose_y print "pose_z: ", self.m_pose_z print "ori_w: ", self.m_ori_w print "ori_x: ", self.m_ori_x print "ori_y: ", self.m_ori_y print "ori_z: ", self.m_ori_z ''' m_pose_goal = Pose() m_pose_goal.orientation = Quaternion(*quaternion_from_euler(0, -1.5, 0)) m_pose_goal.position.x = self.m_pose_x[0] + 0.029# red line 0.2 0.2 m_pose_goal.position.y = self.m_pose_y[0] - 0.7 # green line 0.15 0.15 m_pose_goal.position.z = self.m_pose_z[0] + 0.3 # blue line # 0.35 0.6 ''' print "goal_pose: ", m_pose_goal self.robot_arm.set_pose_target(m_pose_goal) self.robot_arm.go(True) print "==== check check" marker_joint_goal = [0.07100913858593216, -1.8767615298285376, 2.0393206555899503, -1.8313959190971882, -0.6278395875738125, 1.6918219826764682] self.robot_arm.go(marker_joint_goal, wait=True) print "====== robot joint value: \n" print marker_joint_goal #self.robot_arm.go(marker_joint_goal, wait=True) pos_x = self.marker[0].pose.pose.position.x pos_y = self.marker[0].pose.pose.position.y pos_z = self.marker[0].pose.pose.position.z dist = math.sqrt( (pos_x*pos_x) + (pos_y * pos_y) ) ''' #ori_x = marker.pose.pose.orientation.x #ori_y = marker.pose.pose.orientation.y #ori_z = marker.pose.pose.orientation.z #ori_w = marker.pose.pose.orientation.w #print(m_id) #print "===========" #print 'id: ', m_id #print 'pos: ', pos_x, pos_y, pos_z #print('ori: ', ori_x, ori_y, ori_z, ori_w) def move_code(self): planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.wpose = self.robot_arm.get_current_pose() print"====== current pose : ", self.wpose #self.pose_goal = self.robot_arm.get_current_pose() self.pose_goal.position.x = 0.062 self.pose_goal.position.y = 0.194 self.pose_goal.position.z = 0.878 self.pose_goal.orientation.x = 0.673 self.pose_goal.orientation.y = 0.673 self.pose_goal.orientation.z = -0.217 self.pose_goal.orientation.w = 0.217 #print "========== goal frame: ", self.pose_goal self.robot_arm.set_pose_target(self.pose_goal) self.robot_arm.go(True) def look_up_down(self): self.clear_octomap() print "======== clear_octomap" look_up_down = self.robot_arm.get_current_joint_values() #print "before: ", look_up_down look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2 self.robot_arm.go(look_up_down, wait=True) look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2 self.robot_arm.go(look_up_down, wait=True) def plan_cartesian_path(self, scale = 1.0): waypoints = [] ii = 1 self.wpose = self.robot_arm.get_current_pose().pose #self.wpose.position.z -= scale * (0.695) # First move up (z) #self.wpose.position.y += scale * (0.199) # and sideways (y) #waypoints.append(copy.deepcopy(self.wpose)) print "===== robot arm pose: ", self.wpose ''' # 19.06.11 10:45 분 코딩 print self.wpose.position.x," + (",scale," * ",self.goal_x,")" print self.wpose.position.y," + (",scale," * ",self.goal_y,")" self.wpose.position.x = self.wpose.position.x - (scale * self.goal_x) self.wpose.position.y = self.wpose.position.y + (scale * self.goal_y) print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 print self.wpose.position.z," + (",scale," * ",self.goal_z,")" self.wpose.position.z += (scale * self.goal_z) print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ''' self.wpose.position.x = (scale * self.wpose.position.x) - 0.10 #self.wpose.position.y = (scale * self.goal_y) - 0.1 #print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]" waypoints.append(copy.deepcopy(self.wpose)) ii += 1 #print self.wpose.position.z," + (",scale," * ",self.goal_z,")" #self.wpose.position.z = (scale * self.goal_z) + 0.1 self.wpose.position.y = (scale * self.goal_y) + 0.05 waypoints.append(copy.deepcopy(self.wpose)) ii += 1 (plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0) return plan, fraction def execute_plan(self, plan): group = self.robot_arm group.execute(plan, wait=True) def print_ar_pose(self): rospy.loginfo("Waiting for ar_pose_marker topic...") rospy.wait_for_message('ar_pose_marker', AlvarMarkers) rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback) rospy.loginfo("Maker messages detected. Starting followers...") def go_to_move(self, scale = 1.0): self.wpose = self.robot_arm.get_current_pose() planning_frame = self.robot_arm.get_planning_frame() print "========== plannig frame: ", planning_frame self.pose_goal.position.x = (scale * self.goal_x) + 0.10 # base_link to wrist2 x-offset self.pose_goal.position.y = (scale * self.goal_y) - 0.25 self.pose_goal.position.z = (scale * self.goal_z) + 0.72 - 0.115 # world to base_link z-offset and coke can offset self.pose_goal.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.54)) ''' 19.06.1 15:58 코딩 self.pose_goal.orientation.x = self.rot[0] self.pose_goal.orientation.y = self.rot[1] self.pose_goal.orientation.z = self.rot[2] self.pose_goal.orientation.w = self.rot[3] ''' #self.pose_goal.orientation.x = self.goal_roll + 1.57 #self.pose_goal.orientation.y = self.goal_pitch + 1.57 #self.pose_goal.orientation.z = self.goal_yaw print "========== goal frame: ", self.pose_goal self.robot_arm.set_pose_target(self.pose_goal) #self.robot_arm.go(True) def tf_listener(self): # 수정사안: ar_pose id의 가져와서 위치 값을 읽어와야함. try: child_tf = '/ar_marker_9' parent_tf = '/base_link' (self.trans,self.rot) = self.listener.lookupTransform(parent_tf, child_tf, rospy.Time(0)) # rosrun tf tf_echo /base_link /ar_marker_9 랑 같은 역할 # transformation: linear transformation, rotation: quaternion print "======= trans[x, y, z]: ", self.trans print "======= rotat[x, y, z, w]: ", self.rot (self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.rot) print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw #convert 확인용 #quat = quaternion_from_euler(goal_roll, goal_pitch, goal_yaw) #print quat self.goal_x = self.trans[0] self.goal_y = self.trans[1] self.goal_z = self.trans[2] except Exception as ex: print "======== tf_listener Exception!!" rospy.loginfo(ex) def look_object(self): try: look_object = self.robot_arm.get_current_joint_values() #look_object[4] = look_object[4] + (math.radians(90)) # wrist2 look_object[5] = look_object[5] + (3.1) # wrist3 #look_object[3] = look_object[3] - (math.radians(00)) # wrist1 self.robot_arm.go(look_object, wait=True) #look_object[5] = look_object[5] + (math.radians(90)) # wrist3 #self.robot_arm.go(look_object, wait=True) except Exception as ex: print "======== look_object Exception!!" rospy.loginfo(ex)