def main(): rospy.init_node("bottle_shake") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.3) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) # 何かを掴んでいた時のためにハンドを開く gripper.set_joint_value_target([0.9, 0.9]) gripper.go() # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 掴む準備をする target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.0 target_pose.position.z = 0.3 q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを開く gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 掴みに行く target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.0 target_pose.position.z = 0.1 q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを閉じる gripper.set_joint_value_target([0.4, 0.4]) #掴むobjectによって変更する gripper.go() # 持ち上げる target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.0 target_pose.position.z = 0.3 q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 arm.set_named_target("vertical") arm.go() # with open('swing_object.csv') as f: #csvファイルを読み込む # reader = csv.reader(f)#, quoting=csv.QUOTE_NONNUMERIC) # data = [row for row in reader] # data = [[["a",1,20,0.3]],[["a",2,20,0.3]],[["a",3,-60,0.9],["a",5,0,0.9]],[["a",3,1,0.3],["a",5,30,0.3]],[["a",3,-60,0.9],["a",5,-30,0.9]],[["a",3,1,0.3],["a",5,30,0.3]]] data = [[["a",1,20,1]],[["a",3,-60,0.4],["a",5,-40,0.4]],[["a",3,1,1],["a",5,40,1]],[["a",3,-60,0.4],["a",5,-40,0.4]],[["a",3,1,1],["a",5,40,1]],[["a",3,1,1],["a",5,40,1]],[["a",3,-60,0.4],["a",5,-40,0.4]],[["a",3,1,1],["a",5,40,1]]] arm_joint_values = arm.get_current_joint_values() for flame in range(len(data)): for joint_data in range(len(data[flame])): part=data[flame][joint_data][0] joint=int(data[flame][joint_data][1]) angle = float(data[flame][joint_data][2])/180.0*math.pi speed =float(data[flame][joint_data][3]) print(part, joint, angle, speed) if part == "a": # arm_joint_values = arm.get_current_joint_values() arm_joint_values[joint] = angle # elif part == "g": # gripper_joint_values = gripper.get_current_joint_values() # gripper_joint_values[joint] = angle # gripper.set_joint_value_target(gripper_joint_values) print("flame") print(arm_joint_values) arm.set_joint_value_target(arm_joint_values) arm.set_max_velocity_scaling_factor(speed) arm.go() # gripper.go() print("done")
def main(): # init node rospy.init_node("grasp_commander") # ========== publisher to jamming gripper ========== # grasp_pub = rospy.Publisher('/jamming_grasp', Int32, queue_size=1) # ========== Moveit init ========== # # moveit_commander init robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm_initial_pose = arm.get_current_pose().pose target_pose = geometry_msgs.msg.Pose() # Set the planning time arm.set_planning_time(10.0) # ========== TF Lister ========== # tf_buffer = tf2_ros.Buffer() tf_listner = tf2_ros.TransformListener(tf_buffer) for i in [0, 1]: target = "object_" + str(i) get_tf_flg = False # Get target TF while not get_tf_flg: try: trans = tf_buffer.lookup_transform('world', target, rospy.Time(0), rospy.Duration(10)) print "world -> ar_marker" print trans.transform print "Target Place & Pose" # Go to up from target target_pose.position.x = trans.transform.translation.x target_pose.position.y = trans.transform.translation.y target_pose.position.z = trans.transform.translation.z + 0.10 q = (trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w) (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(q) # roll -= math.pi/6.0 pitch += math.pi / 2.0 # yaw += math.pi/4.0 tar_q = tf.transformations.quaternion_from_euler( roll, pitch, yaw) target_pose.orientation.x = tar_q[0] target_pose.orientation.y = tar_q[1] target_pose.orientation.z = tar_q[2] target_pose.orientation.w = tar_q[3] print target_pose arm.set_pose_target(target_pose) arm.go() arm.clear_pose_targets() #rospy.sleep(2) # Get Grasp # waypoints = [] # waypoints.append(arm.get_current_pose().pose) # wpose = geometry_msgs.msg.Pose() # wpose.orientation.w = 1.0 # wpose.position.x = waypoints[0].position.x # wpose.position.y = waypoints[0].position.y - 0.085 # wpose.position.z = waypoints[0].position.z # waypoints.append(copy.deepcopy(wpose)) # (plan, fraction) = arm.compute_cartesian_path(waypoints, 0.005, 0.0, False) # target_pose.position.x = trans.transform.translation.x # target_pose.position.y = trans.transform.translation.y + 0.04 # target_pose.position.z = trans.transform.translation.z + 0.243 # target_pose.orientation = target_pose.orientation # print target_pose # arm.set_pose_target(target_pose) # arm.go() # arm.clear_pose_targets() # # Grasp # grasp_pub.publish(1) # print "!! Grasping !!" # rospy.sleep(2.0) # # Start Return # # waypoints = [] # # waypoints.append(arm.get_current_pose().pose) # # wpose = geometry_msgs.msg.Pose() # # wpose.orientation.w = 1.0 # # wpose.position.x = waypoints[0].position.x # # wpose.position.y = waypoints[0].position.y + 0.085 # # wpose.position.z = waypoints[0].position.z # # waypoints.append(copy.deepcopy(wpose)) # # (plan, fraction) = arm.compute_cartesian_path(waypoints, 0.005, 0.0, False) # target_pose.position.x = trans.transform.translation.x # target_pose.position.y = trans.transform.translation.y + 0.04 # target_pose.position.z = trans.transform.translation.z + 0.32 # target_pose.orientation = target_pose.orientation # print target_pose # arm.set_pose_target(target_pose) # arm.go() # arm.clear_pose_targets() # #rospy.sleep(2) # # Go to Home Position # target_pose.position.x = -0.13683 + (i-1)*0.08 # target_pose.position.y = -0.22166 # target_pose.position.z = 0.50554 # target_pose.orientation.x = 0.00021057 # target_pose.orientation.y = 0.70092 # target_pose.orientation.z = 0.00030867 # target_pose.orientation.w = 0.71324 # arm.set_pose_target(target_pose) # arm.go() # arm.clear_pose_targets() # #rospy.sleep(2) # target_pose.position.x = target_pose.position.x # target_pose.position.y = target_pose.position.y # target_pose.position.z -= 0.10 # target_pose.orientation = target_pose.orientation # arm.set_pose_target(target_pose) # arm.go() # arm.clear_pose_targets() # # Release # print " !! Release !!" # grasp_pub.publish(0) # rospy.sleep(1) get_tf_flg = True except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue
from gazebo_msgs.msg import LinkStates from std_srvs.srv import Empty from gazebo_msgs.msg import LinkState from gazebo_msgs.srv import SetLinkState from rosgraph_msgs.msg import Clock import moveit_commander import moveit_msgs.msg import geometry_msgs.msg from std_msgs.msg import String from moveit_commander.conversions import pose_to_list moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('trajectory_planning', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = "aarm_group" move_group = moveit_commander.MoveGroupCommander(group_name) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) planning_frame = move_group.get_planning_frame() #print ("============ Planning frame: %s" % planning_frame) #eef_link = move_group.get_end_effector_link() #print "============ End effector link: %s" % eef_link
def __init__(self): # print("__init__ of GazeboSmartBotPincherKinectEnv") # Launch the simulation with the given launch file name # launch file is in gym-gazebo/gym_gazebo/env/assets/launch/ gazebo_env.GazeboEnv.__init__(self, "GazeboSmartBotPincherKinect_v0.launch") self.link_state_pub = rospy.Publisher("/gazebo/set_link_state", LinkState, queue_size=5) self.unpause_proxy = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) self.pause_proxy = rospy.ServiceProxy("/gazebo/pause_physics", Empty) self.reset_proxy = rospy.ServiceProxy("/gazebo/reset_simulation", Empty) # the state space (=observation space) are all possible depth images of the kinect camera # Test for HER algorithm self.observation_space = spaces.Dict( dict( desired_goal=spaces.Box(shape=(6, ), dtype='float32'), achieved_goal=spaces.Box(shape=(6, ), dtype='float32'), observation=spaces.Box(low=0, high=255, shape=[220 * 300], dtype=np.uint8), )) # the action space are all possible positions & orientations (6-DOF), # which are bounded in the area in front of the robot arm where an object can lie (see reset()) boundaries_xAxis = [0.04, 0.3] # box position possiblities: (0.06, 0.22) boundaries_yAxis = [-0.25, 0.25] # box position possiblities: (-0.2, 0.2) boundaries_zAxis = [0, 0.05] # box position possiblities: (0.05, 0.1) boundaries_roll = [0, 2 * np.pi] # box position possiblities: 0 boundaries_pitch = [0, 2 * np.pi] # box position possiblities: 0 boundaries_yaw = [0, 2 * np.pi] # box position possiblities: (0, np.pi) # test with symmetric boundaries # boundaries_xAxis = [-0.3, 0.3] # boundaries_yAxis = [-0.25, 0.25] # boundaries_zAxis = [-0.5, 0.5] # boundaries_roll = [-np.pi, np.pi] # boundaries_pitch = [-np.pi, np.pi] # boundaries_yaw = [-np.pi, np.pi] low = np.array([ boundaries_xAxis[0], boundaries_yAxis[0], boundaries_zAxis[0], boundaries_roll[0], boundaries_pitch[0], boundaries_yaw[0] ]) high = np.array([ boundaries_xAxis[1], boundaries_yAxis[1], boundaries_zAxis[1], boundaries_roll[1], boundaries_pitch[1], boundaries_yaw[1] ]) self.action_space = spaces.Box(low=low, high=high, dtype=np.float32) self.reward_range = (-np.inf, np.inf) self.seed() # setting up the scene print("inserting table plane") self.insertObject( 0.4, 0, 0.025 / 2, "/home/joel/Documents/pluginTest/smartBot_plugin/models/tablePlane/tablePlane.sdf", "tablePlane") print("inserting kinect sensor") self.insertObject( 0.03, 0, 0.5, "/home/joel/Documents/pluginTest/smartBot_plugin/models/kinect_ros/model.sdf", "kinect_ros", roll=0, pitch=np.pi / 2, yaw=0) print("inserting object to pick up") self.insertObject( 0.1, 0, 0.1, "/home/joel/Documents/pluginTest/smartBot_plugin/models/objectToPickUp/objectToPickUp.sdf", "objectToPickUp") # initializing MoveIt variables # it's necessary to unpause the simulation in order to initialize the MoveGroupCommander, printing robot state etc. self.unpause_simulation() moveit_commander.roscpp_initialize(sys.argv) # rospy.init_node("move_group_python_interface", anonymous=True) # can't init another node, there is already one initialized in the superclass self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group_arm = moveit_commander.MoveGroupCommander("arm") # used to be: base_link, also possible: world (if defined in the urdf.xarco file) self.REFERENCE_FRAME = "world" # Allow some leeway in position (meters) and orientation (radians) self.group_arm.set_goal_position_tolerance(0.01) self.group_arm.set_goal_orientation_tolerance(0.01) # Allow replanning to increase the odds of a solution self.group_arm.allow_replanning(True) # Set the right arm reference frame self.group_arm.set_pose_reference_frame(self.REFERENCE_FRAME) # Allow 5 seconds per planning attempt self.group_arm.set_planning_time(5) self.bridge = CvBridge() self.pause_simulation()
def __init__(self): super(MoveGroupPythonIntefaceTutorial, self).__init__() ## BEGIN_SUB_TUTORIAL setup ## ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python', anonymous=True) ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's ## kinematic model and the robot's current joint states robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface ## for getting, setting, and updating the robot's internal understanding of the ## surrounding world: scene = moveit_commander.PlanningSceneInterface() ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to a planning group (group of joints). In this tutorial the group is the primary ## arm joints in the Panda robot, so we set the group's name to "panda_arm". ## If you are using a different robot, change this value to the name of your robot ## arm planning group. ## This interface can be used to plan and execute motions: group_name = "iiwa" move_group = moveit_commander.MoveGroupCommander(group_name) ## Create a `DisplayTrajectory`_ ROS publisher which is used to display ## trajectories in Rviz: display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) ## END_SUB_TUTORIAL ## BEGIN_SUB_TUTORIAL basic_info ## ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ # 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 "" ## END_SUB_TUTORIAL # Misc variables self.base_name = 'base' self.w0_name = 'wheel0' self.w1_name = 'wheel1' self.top_name = 'top' self.robot = robot self.scene = scene self.move_group = move_group self.display_trajectory_publisher = display_trajectory_publisher self.planning_frame = planning_frame self.eef_link = eef_link self.group_names = group_names self.base_pose = geometry_msgs.msg.PoseStamped() self.w0_pose = geometry_msgs.msg.PoseStamped() self.w1_pose = geometry_msgs.msg.PoseStamped() self.top_pose = geometry_msgs.msg.PoseStamped() self.aruco_pose = geometry_msgs.msg.PoseStamped() self.aruco_res = geometry_msgs.msg.PoseStamped()
def movo_moveit_test(): ## BEGIN_TUTORIAL ## ## Setup ## ^^^^^ ## CALL_SUB_TUTORIAL imports ## ## First initialize moveit_commander and rospy. print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('movo_moveit_test', anonymous=True) is_sim = rospy.get_param('~sim', False) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. scene = moveit_commander.PlanningSceneInterface() ##Gripper action clients lgripper = GripperActionTest('left') rgripper = GripperActionTest('right') ## Instantiate a MoveGroupCommander object. This object is an interface ## to one group of joints. In this case the group is the joints in the left ## arm. This interface can be used to plan and execute motions on the left ## arm. group = moveit_commander.MoveGroupCommander("upper_body") ## We create this DisplayTrajectory publisher which is used below to publish ## trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) rospy.sleep(2) scene.remove_world_object("floor") # publish a demo scene p = geometry_msgs.msg.PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.01 p.pose.orientation.w = 1.0 scene.add_box("floor", p, (4.0, 4.0, 0.02)) ## We can get a list of all the groups in the robot print "============ Robot Groups:" print 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 "============" group.set_planner_id("RRTConnectkConfigDefault") ## Planning to a Pose goal ## ^^^^^^^^^^^^^^^^^^^^^^^ ## We can plan a motion for this group to a desired pose for the ## end-effector if (True == is_sim): gripper_closed = 0.96 gripper_open = 0.0 else: gripper_closed = 0.0 gripper_open = 0.165 print "============ Testing with named targets" group.set_named_target("homed") plan = group.plan() group.execute(plan) lgripper.command(gripper_open) rgripper.command(gripper_open) lgripper.wait() rgripper.wait() rospy.sleep(2.0) group.set_named_target("tucked") plan = group.plan() group.execute(plan) lgripper.command(gripper_closed) rgripper.command(gripper_closed) lgripper.wait() rgripper.wait() rospy.sleep(2.0) group.set_named_target("pos1") plan = group.plan() group.execute(plan) lgripper.command(gripper_open) rgripper.command(gripper_open) lgripper.wait() rgripper.wait() rospy.sleep(2.0) group.set_named_target("tucked") plan = group.plan() group.execute(plan) lgripper.command(gripper_closed) rgripper.command(gripper_closed) lgripper.wait() rgripper.wait() ## When finished shut down moveit_commander. moveit_commander.roscpp_shutdown() ## END_TUTORIAL print "============ STOPPING"
def main(): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node("joint_state_player_node") waypoint_file = rospy.get_param('wpt_file', 'scan_waypoints2.npy') limb_name = rospy.get_param('limb_name', 'left_arm') robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() arm = moveit_commander.MoveGroupCommander(limb_name) waypoints = np.load(waypoint_file) #calib_sim3.npy #calib_real_right03.npy rate = rospy.Rate(100) joint_angle_target = np.zeros(7) selected_idx = -1 while not rospy.is_shutdown(): c = getch() if c in ['n']: selected_idx = (selected_idx + 1) % len(waypoints) joint_angle_target = waypoints[selected_idx] print("Selected joint goal %d: " % (selected_idx, ), joint_angle_target) elif c in ['b']: selected_idx -= 1 if selected_idx < 0: selected_idx = len(waypoints) - 1 joint_angle_target = waypoints[selected_idx] print("Selected joint goal %d: " % (selected_idx, ), joint_angle_target) elif c in ['a']: current_joint_angles = arm.get_current_joint_values() joint_angles = np.asarray(current_joint_angles) waypoints = np.vstack([waypoints, joint_angles]) print("Added new waypoint. Number of waypoints %d." % (len(waypoints, ))) print("Joint angles: ", joint_angles) elif c in ['d']: if len(waypoints) > 0: waypoints = np.delete(waypoints, (selected_idx), axis=0) print("Deleted %dth waypoint. Number of waypoints %d." % (selected_idx + 1, len(waypoints, ))) else: print("Waypoint list is empty.") elif c in ['e']: current_joint_angles = np.asarray(arm.get_current_joint_values()) before = np.array(waypoints[selected_idx]) waypoints[selected_idx] = current_joint_angles print("Editting joint angles %d: " % (selected_idx, ), before) print("Set to", current_joint_angles) elif c in ['s']: print("Saving current waypoint list.") filename = raw_input("Filename:") np.save('%s.npy' % (filename, ), waypoints) print("Saved.") elif c in ['g']: try: arm.set_joint_value_target(joint_angle_target) arm.set_max_velocity_scaling_factor(0.35) arm.set_max_acceleration_scaling_factor(0.35) arm.go() except: print("Not possible to go to waypoint. Try another one") elif c in ['\x1b', '\x03']: rospy.signal_shutdown("Bye.") rate.sleep() moveit_commander.roscpp_shutdown()
def move_group_python_interface_tutorial(): ## First initialize moveit_commander and rospy. print "============ Starting setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('collision_checker','move_group_python_interface_tutorial', anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. scene = moveit_commander.PlanningSceneInterface() ## Instantiate a MoveGroupCommander object. This object is an interface ## to one group of joints. In this case the group is the joints in the left ## arm. This interface can be used to plan and execute motions on the left ## arm. group = moveit_commander.MoveGroupCommander("manipulator") #group.set_goal_position_tolerance(0.01) #group.set_goal_orientation_tolerance(0.1) group.allow_replanning(True) group.set_planner_id("ESTkConfigDefault") #RRTConnectkConfigDefault/SBLkConfigDefault/KPIECEkConfigDefault/BKPIECEkConfigDefault/LBKPIECEkConfigDefault/ ESTkConfigDefault #group.set_planning_time(15) group.set_num_planning_attempts(5) replanning_num = 5 ## We create this DisplayTrajectory publisher which is used below to publish ## trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ ## ## We can get the name of the reference frame for this robot print "============ Reference frame: %s" % group.get_planning_frame() ## Sometimes for debugging it is useful to print the entire state of the ## robot. print "============ Printing robot state" print robot.get_current_state() print "============" print "============ Printing robot Pose" print group.get_current_pose().pose print "============" ## Set up the scene rospack = rospkg.RosPack() scene_path = rospack.get_path('abb_irb6640_support') wall_1 = os.path.join(scene_path,'meshes/Scene/T1.stl') wall_2 = os.path.join(scene_path,'meshes/Scene/T1.stl') wall_3 = os.path.join(scene_path,'meshes/Scene/T3.stl') Ceiling = os.path.join(scene_path,'meshes/Scene/TestBed_scene5.STL') # Set the target pose in between the boxes and on the table target_pose_panel = gen_pose_scene([1.862,0,1.325,0.5,-0.5,-0.5,0.5])#[0,-2,0.35,0.5,-0.5,-0.5,0.5] target_pose_panel.header.frame_id = 'base_link' #target_pose_wall_1 = gen_pose_scene([1.5384, -3.8262, 1,0,0,0,1]) #target_pose_wall_2 = gen_pose_scene([1.5384, 2.6, 1,0,0,0,1]) #target_pose_wall_3 = gen_pose_scene([-0.9, -0.6131, 1,0,0,0,1]) #target_pose_Ceiling = gen_pose_scene([-0.9, -4.377, 0, 0,-0.707, -0.707,0]) print "============ Starting Simulation ============" x_All = [] y_All = [] Dist_Mean_All = [] Dist_Min_All = [] cc = CollisionChecker(gui=False) for i_x in np.linspace(-1,-0.72,8): #-0.68,0.20,23 for i_y in np.linspace(-1.2,1.2,51): #(-1,1,51) scene.remove_world_object('target') #scene.remove_world_object('Wall_1') #scene.remove_world_object('Wall_2') #scene.remove_world_object('Wall_3') #scene.remove_world_object('Ceiling') rospy.sleep(2) print "============ Iteration ============" print i_x, i_y np.savetxt('ScoreMap.out',(x_All,y_All,Dist_Mean_All,Dist_Min_All)) Dist = [] robot_pose = [i_x, i_y, 0, 0, 0, 0, 1] set_robot_pose(robot_pose) # Add the target object to the scene scene.add_box('target', target_pose_panel, [2.33, 0.2, 2.08] ) #scene.add_mesh('Wall_1', target_pose_wall_1, wall_1 , size =(0.5,0.5,0.5)) #scene.add_mesh('Wall_2', target_pose_wall_2, wall_2 , size =(0.5,0.5,0.5)) #scene.add_mesh('Wall_3', target_pose_wall_3, wall_3 , size =(0.5,0.5,0.5)) #scene.add_mesh('Ceiling', target_pose_Ceiling, Ceiling) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(2) print "============ Generating plan REST ============" group.clear_pose_targets() pose_target = gen_pose_target([1.862,0,1.955,0,-1,0,0]) group.set_pose_target(pose_target) planR = group.plan() cnt = 0; while( (not planR.joint_trajectory.points) & (cnt< replanning_num)): cnt = cnt+1; print "============ Generating plan again" planR = group.plan() if (cnt == 5): Dist=-1 group.detach_object('link_6') x_All.append(i_x) y_All.append(i_y) Dist_Mean_All.append(np.mean(Dist)) Dist_Min_All.append(np.min(Dist)) continue rospy.sleep(1) print "============ Executing plan REST" group.execute(planR) print "============ Waiting while plan REST is Executed..." rospy.sleep(1) T1 = np.dot(translation_matrix(robot_pose[0:3]), quaternion_matrix(robot_pose[3:7])) T2 = np.dot(translation_matrix([0,0,0]), quaternion_matrix([0,0,0,1])) T3 = np.dot(T1,np.dot(translation_matrix([0,-2,0.35]), quaternion_matrix([0.5, 0.5, 0.5, 0.5]))) group.attach_object('target','link_6') rospy.sleep(2) collision_env = { 'Walls' : T2 } print "============ Generating plan 2 ============" group.clear_pose_targets() pose_target = gen_pose_target([1.7-robot_pose[0],-1.165-robot_pose[1],0.62,0,-1,0,0]) group.set_pose_target(pose_target) ## Now, we call the planner to compute the plan ## and visualize it if successful ## Note that we are just planning, not asking move_group ## to actually move the robot plan2 = group.plan() cnt = 0; while( (not plan2.joint_trajectory.points) & (cnt< replanning_num)): cnt = cnt+1; print "============ Generating plan again" plan2 = group.plan() if (cnt == 5): Dist=-1 group.detach_object('link_6') x_All.append(i_x) y_All.append(i_y) Dist_Mean_All.append(np.mean(Dist)) Dist_Min_All.append(np.min(Dist)) continue #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ for i_plan in range(0,len(plan2.joint_trajectory.points)): tmp_array = plan2.joint_trajectory.points[i_plan].positions T3 = np.dot(T1,np.dot(FK_Matrix2(tmp_array), quaternion_matrix([0.707, 0.707, 0, 0]))) #T3 = FK_Matrix(tmp_array) joints = { 'irb6640_185_280_Testbed' : tmp_array } collision_poi = { 'irb6640_185_280_Testbed' : T1, 'box' : T3 } tmp_result = cc.check_safety(collision_poi, collision_env, joints) print 'Safe (0.001 tolerance) =',tmp_result Dist.append(tmp_result[1]) #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ rospy.sleep(1) print "============ Executing plan2" group.execute(plan2) print "============ Waiting while plan2 is Executed..." rospy.sleep(1) print "============ Generating plan REST ============" group.clear_pose_targets() pose_target = gen_pose_target([1.862,0,1.955,0,-1,0,0]) group.set_pose_target(pose_target) planR = group.plan() cnt = 0; while( (not planR.joint_trajectory.points) & (cnt< replanning_num)): cnt = cnt+1; print "============ Generating plan again" planR = group.plan() if (cnt == 5): Dist=-1 group.detach_object('link_6') x_All.append(i_x) y_All.append(i_y) Dist_Mean_All.append(np.mean(Dist)) Dist_Min_All.append(np.min(Dist)) continue #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ for i_plan in range(0,len(planR.joint_trajectory.points)): tmp_array = planR.joint_trajectory.points[i_plan].positions T3 = np.dot(T1,np.dot(FK_Matrix2(tmp_array), quaternion_matrix([0.707, 0.707, 0, 0]))) joints = { 'irb6640_185_280_Testbed' : tmp_array } collision_poi = { 'irb6640_185_280_Testbed' : T1, 'box' : T3 } tmp_result = cc.check_safety(collision_poi, collision_env, joints) print 'Safe (0.001 tolerance) =',tmp_result Dist.append(tmp_result[1]) #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ rospy.sleep(1) print "============ Executing plan REST" group.execute(planR) print "============ Waiting while plan REST is Executed..." rospy.sleep(1) print "============ Generating plan 3 ============" group.clear_pose_targets() pose_target = gen_pose_target([1.7-robot_pose[0],1.165-robot_pose[1],0.62,0,-1,0,0]) group.set_pose_target(pose_target) plan3 = group.plan() cnt = 0; while( (not plan3.joint_trajectory.points) & (cnt< replanning_num)): cnt = cnt+1; print "============ Generating plan again" plan3 = group.plan() if (cnt == 5): Dist=-1 group.detach_object('link_6') x_All.append(i_x) y_All.append(i_y) Dist_Mean_All.append(np.mean(Dist)) Dist_Min_All.append(np.min(Dist)) continue #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ for i_plan in range(0,len(plan3.joint_trajectory.points)): tmp_array = plan3.joint_trajectory.points[i_plan].positions T3 = np.dot(T1,np.dot(FK_Matrix2(tmp_array), quaternion_matrix([0.707, 0.707, 0, 0]))) joints = { 'irb6640_185_280_Testbed' : tmp_array } collision_poi = { 'irb6640_185_280_Testbed' : T1, 'box' : T3 } tmp_result = cc.check_safety(collision_poi, collision_env, joints) print 'Safe (0.001 tolerance) =',tmp_result Dist.append(tmp_result[1]) #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ rospy.sleep(1) print "============ Executing plan3" group.execute(plan3) print "============ Waiting while plan3 is Executed..." rospy.sleep(1) print "============ Generating plan REST ============" group.clear_pose_targets() pose_target = gen_pose_target([1.862,0,1.955,0,-1,0,0]) group.set_pose_target(pose_target) planR = group.plan() cnt = 0; while( (not planR.joint_trajectory.points) & (cnt< replanning_num)): cnt = cnt+1; print "============ Generating plan again" planR = group.plan() if (cnt == 5): Dist=-1 group.detach_object('link_6') x_All.append(i_x) y_All.append(i_y) Dist_Mean_All.append(np.mean(Dist)) Dist_Min_All.append(np.min(Dist)) continue #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ for i_plan in range(0,len(planR.joint_trajectory.points)): tmp_array = planR.joint_trajectory.points[i_plan].positions T3 = np.dot(T1,np.dot(FK_Matrix2(tmp_array), quaternion_matrix([0.707, 0.707, 0, 0]))) joints = { 'irb6640_185_280_Testbed' : tmp_array } collision_poi = { 'irb6640_185_280_Testbed' : T1, 'box' : T3 } tmp_result = cc.check_safety(collision_poi, collision_env, joints) print 'Safe (0.001 tolerance) =',tmp_result Dist.append(tmp_result[1]) #++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ rospy.sleep(1) print "============ Executing plan REST" group.execute(planR) print "============ Waiting while plan REST is Executed..." rospy.sleep(1) ## Adding/Removing Objects and Attaching/Detaching Objects ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## First, we will define the collision object message group.detach_object('link_6') x_All.append(i_x) y_All.append(i_y) Dist_Mean_All.append(np.mean(Dist)) Dist_Min_All.append(np.min(Dist)) ## END scene.remove_world_object('target') #scene.remove_world_object('Wall_1') #scene.remove_world_object('Wall_2') #scene.remove_world_object('Wall_3') #scene.remove_world_object('Ceiling') rospy.sleep(2) print "============ STOPPING" collision_object = moveit_msgs.msg.CollisionObject() np.savetxt('ScoreMap.out',(x_All,y_All,Dist_Mean_All,Dist_Min_All)) ## When finished shut down moveit_commander. moveit_commander.roscpp_shutdown()
def main(): rospy.init_node("crane_x7_pick_and_place_controller") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.1) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) # 何かを掴んでいた時のためにハンドを開く gripper.set_joint_value_target([0.9, 0.9]) gripper.go() # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 掴む準備をする target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.0 target_pose.position.z = 0.3 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを開く gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 掴みに行く target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.0 target_pose.position.z = 0.13 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを閉じる gripper.set_joint_value_target([0.4, 0.4]) gripper.go() # 持ち上げる target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.0 target_pose.position.z = 0.3 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # 移動する target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.2 target_pose.position.z = 0.3 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # 下ろす target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.2 target_pose.position.z = 0.13 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # ハンドを開く gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 少しだけハンドを持ち上げる target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.2 target_pose.position.y = 0.2 target_pose.position.z = 0.2 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() print("done")
def main(): rospy.init_node("pose_groupstate_example") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(0.1) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) # 何かを掴んでいた時のためにハンドを開く gripper.set_joint_value_target([0.9, 0.9]) gripper.go() # SRDFに定義されている"home"の姿勢にする print("home") arm.set_named_target("home") arm.go() # SRDFに定義されている"vertical"の姿勢にする print("vertical") arm.set_named_target("vertical") arm.go() # ハンドを少し閉じる gripper.set_joint_value_target([0.7, 0.7]) gripper.go() # 手動で姿勢を指定するには以下のように指定 gripper.set_joint_value_target([0.9, 0.9]) gripper.go() target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.0 target_pose.position.y = 0.0 target_pose.position.z = 0.0 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2) # 上方から掴み>に行く場合 target_pose = geometry_msgs.msg.Pose() target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() gripper.set_joint_value_target([0.7, 0.7]) gripper.go() print("あああああああああああ") # 移動後の手先ポーズを表示 arm.set_named_target("home") arm.go() print("done")
def __init__(self, log_level): rospy.init_node('cb_moveit_gui_execution', log_level=log_level) # Subscriber # rospy.Subscriber("clicked_point", PointStamped, self.clicked_cb) rospy.Subscriber("pick_approach_plan", Bool, self.pick_approach_plan_cb) rospy.Subscriber("pick_approach_execute", Bool, self.pick_approach_execute_cb) rospy.Subscriber("pick_retreat_plan", Bool, self.pick_retreat_plan_cb) rospy.Subscriber("pick_retreat_execute", Bool, self.pick_retreat_execute_cb) rospy.Subscriber("place_approach_plan", Bool, self.place_approach_plan_cb) rospy.Subscriber("place_approach_execute", Bool, self.place_approach_execute_cb) rospy.Subscriber("place_retreat_plan", Bool, self.place_retreat_plan_cb) rospy.Subscriber("place_retreat_execute", Bool, self.place_retreat_execute_cb) rospy.Subscriber("approach_stop", Bool, self.approach_stop_cb) rospy.Subscriber("move_xp", Bool, self.move_xp_cb) rospy.Subscriber("move_xm", Bool, self.move_xm_cb) rospy.Subscriber("move_yp", Bool, self.move_yp_cb) rospy.Subscriber("move_ym", Bool, self.move_ym_cb) rospy.Subscriber("move_zp", Bool, self.move_zp_cb) rospy.Subscriber("move_zm", Bool, self.move_zm_cb) rospy.Subscriber("waypoints/update", InteractiveMarkerUpdate, self.waypoints_update_cb) rospy.Subscriber("distance", Int32, self.distance_cb) rospy.Subscriber("solution_num", Int32, self.solution_cb) rospy.Subscriber("pcl_record", Bool, self.record_move_cb) rospy.Subscriber("compute_interpolation", Bool, self.stitch_plan_cb) rospy.Subscriber("/cb_gui_moveit/pcl_capture", Bool, self.pcl_capture_cb) rospy.Subscriber("/cb_gui_moveit/pcl_clear", Bool, self.pcl_clear_cb) rospy.Subscriber("/cb_gui_moveit/grasp_object", Bool, self.grasp_object_cb) rospy.Subscriber("/cb_gui_moveit/grasp_wand", Bool, self.grasp_wand_cb) rospy.Subscriber("/cb_gui_moveit/scanning_plan", Bool, self.scanning_plan_req) rospy.Subscriber("/cb_gui_moveit/scanning_execute", Bool, self.scanning_execute_req) rospy.Subscriber("/cb_gui_moveit/j1", Bool, self.move_j1_cb) rospy.Subscriber("/cb_gui_moveit/j4", Bool, self.move_j4_cb) rospy.Subscriber("/cb_gui_moveit/j6", Bool, self.move_j6_cb) # Publisher self.instruction_pub = rospy.Publisher('/instruction', String, queue_size=1) self.aco_pub = rospy.Publisher('/attached_collision_object', moveit_msgs.msg.AttachedCollisionObject, queue_size=100) self.pcl_capture_pub = rospy.Publisher('/pcl_capture', Bool, queue_size=1) self.remove_imarker_pub = rospy.Publisher("/remove_imarker", Bool, queue_size=1) self.gripper_close_pub = rospy.Publisher("/gripper_close", Bool, queue_size=1) # Service rospy.wait_for_service('/pcl_fusion_node/reset') rospy.Service("/cb_gui_moveit/scanning_plan", Trigger, self.scanning_plan_req) rospy.Service("/cb_gui_moveit/scanning_execute", Trigger, self.scanning_execute_req) # ready for listen tf self.listener = tf.TransformListener() # moveit commander self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group = moveit_commander.MoveGroupCommander("ur5") self.group.set_planner_id("RRTConnectkConfigDefault") self.group.set_planning_time(5.0) self.group.set_num_planning_attempts(10) self.group.set_max_velocity_scaling_factor(0.05) # 0.1 self.group.set_max_acceleration_scaling_factor(0.05) # 0.1 self.pick_approach_plan = None self.pick_retreat_plan = None self.pick_retreat_plan1 = None self.place_approach_plan = None self.place_retreat_plan = None # for compute cartesian path self.scanning_plan1 = None self.scanning_plan2 = None self.jump_threshold = 0.0 self.eef_step = 0.01 # set after execution self.pre_grasp_joint = None self.post_grasp_joint = None self.pick_retreat_step = 0 self.last_offset = 10.0 # IK self.ur5_inv = ur5_inv_kin_wrapper() self.last_selected_joint = None self.last_sol_num = None self.count = 0 # initialize # from capture #self.initialize(initial_joint_for_capture) #self.grasp_wand = False # from scan self.initialize(initial_joint_for_scan) self.grasp_wand = True
def __init__(self): print("__init__ of GazeboSmartBotPincherKinectEnvREAL_ARM") # Launch the moveit config for the real pincher arm # launch file is in gym-gazebo/gym_gazebo/env/assets/launch/ gazebo_env.GazeboEnv.__init__( self, "GazeboSmartBotPincherKinectREAL_ARM_v0.launch") # the state space (=observation space) are all possible depth images of the kinect camera if (self.flattenImage): self.observation_space = spaces.Box(low=0, high=255, shape=[220 * 300], dtype=np.uint8) else: self.observation_space = spaces.Box(low=0, high=255, shape=[220, 300], dtype=np.uint8) # the action space are all possible positions & orientations (6-DOF), # which are bounded in the area in front of the robot arm where an object can lie (see reset()) boundaries_xAxis = [0.04, 0.3] # box position possiblities: (0.06, 0.22) boundaries_yAxis = [-0.25, 0.25] # box position possiblities: (-0.2, 0.2) boundaries_zAxis = [0.015, 0.05] # box z-position: ca. 0.04 boundaries_roll = [0, 2 * np.pi] boundaries_pitch = [0, 2 * np.pi] boundaries_yaw = [0, 2 * np.pi] low = np.array([ boundaries_xAxis[0], boundaries_yAxis[0], boundaries_zAxis[0], boundaries_roll[0], boundaries_pitch[0], boundaries_yaw[0] ]) high = np.array([ boundaries_xAxis[1], boundaries_yAxis[1], boundaries_zAxis[1], boundaries_roll[1], boundaries_pitch[1], boundaries_yaw[1] ]) self.action_space = spaces.Box(low=low, high=high, dtype=np.float32) self.reward_range = (-np.inf, np.inf) self.seed() rospy.sleep(2) # MoveIt variables moveit_commander.roscpp_initialize(sys.argv) # rospy.init_node('move_group_python_interface', anonymous=True) # can't init another node, there is already one initialized in the superclass self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group_arm = moveit_commander.MoveGroupCommander("arm") self.group_gripper = moveit_commander.MoveGroupCommander("gripper") # used to be: base_link, also possible: world (if defined in the urdf.xarco file) self.REFERENCE_FRAME = 'world' # Allow some leeway in position (meters) and orientation (radians) self.group_arm.set_goal_position_tolerance(0.01) self.group_arm.set_goal_orientation_tolerance(0.01) # Allow replanning to increase the odds of a solution self.group_arm.allow_replanning(True) # Set the right arm reference frame self.group_arm.set_pose_reference_frame(self.REFERENCE_FRAME) # Allow 5 seconds per planning attempt self.group_arm.set_planning_time(5) self.bridge = CvBridge()
arm_right_group = moveit_commander.MoveGroupCommander("arm_right_torso"); arm_right_group.set_planner_id("RRTstarkConfigDefault"); #arm_right_group.set_planner_id("RRTConnectkConfigDefault"); arm_right_group.set_goal_tolerance(0.001); arm_right_group.allow_replanning(True); #pos_init(arm_left_group, arm_right_group); arm_left_group.set_planning_time(Planning_time); arm_right_group.set_planning_time(Planning_time); print ">>>> Waiting for service `compute_ik` >>>>"; rospy.wait_for_service('compute_ik'); ik = rospy.ServiceProxy("compute_ik", GetPositionIK); print ">>>> Start Testing >>>>" pos_test(arm_right_group, ik); motoman_sda10 = moveit_commander.RobotCommander(); start_state = motoman_sda10.get_current_state(); goal_state = [0.0, -0.7846225685744268, -0.44391901469000256, 2.0332152364478326, -0.9467777692180627, 1.094702439784921, -1.454522631785723, 2.482112603511986] #constraint_planner(start_state, goal_state, arm_right_group, "RRTConnectkConfigDefault", 15, 10); # w 0.70683; x 0.00024277; y 0.00024296; z 0.70739 # des_w = 0.68323; des_x = -0.68245; des_y = -0.18413; des_z = -0.18316; orient_constraint = setOrientationConstraints(des_w, des_x, des_y, des_z, arm_right_group, weight = 1.0); test_constraints = Constraints(); test_constraints.orientation_constraints.append(orient_constraint); arm_right_group.set_path_constraints(test_constraints); test_jnt_value_msg = Generate_joint_state_msg(arm_right_group,goal_state) arm_right_group.set_joint_value_target(test_jnt_value_msg);
def __init__(self): """ The constructor for Ur5MoveitActionClient. This function will initalize this node as ROS Action Client. Moveit configurations for UR5_1 will also be setup here """ # Configuing Moveit self._robot_ns = '/' + 'ur5_1' self._planning_group = "manipulator" self._commander = moveit_commander.roscpp_initialize(sys.argv) self._robot = moveit_commander.RobotCommander(robot_description= self._robot_ns + "/robot_description", ns=self._robot_ns) self._scene = moveit_commander.PlanningSceneInterface(ns=self._robot_ns) self._group = moveit_commander.MoveGroupCommander(self._planning_group, robot_description= self._robot_ns + "/robot_description", ns=self._robot_ns) self._display_trajectory_publisher = rospy.Publisher( self._robot_ns + '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1) self._exectute_trajectory_client = actionlib.SimpleActionClient( self._robot_ns + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction) self._exectute_trajectory_client.wait_for_server() self._planning_frame = self._group.get_planning_frame() self._eef_link = self._group.get_end_effector_link() self._group_names = self._robot.get_group_names() # Make it a publisher to publish on '/eyrc/vb/onConveyor' self.onConveyor_pub_handle = rospy.Publisher("/eyrc/vb/onConveyor", onConveyor, queue_size = 10) # A dict to store all the processed info on color of pkgs self.package_pose_color = {} # A dict to store all the incoming orders self.orders_to_be_completed = {} # Lists to store order number of their respective priority # this helps to deliver higher priority order first self.orders_to_be_completed_RED = [] self.orders_to_be_completed_YELLOW = [] self.orders_to_be_completed_GREEN = [] self.currently_processing = False # Current State of the Robot is needed to add box to planning scene self._curr_state = self._robot.get_current_state() # Setup to play all the stored trajectories rp = rospkg.RosPack() self._pkg_path = rp.get_path('pkg_task5') self._file_path = self._pkg_path + '/config/saved_trajectories/' rospy.loginfo( "Package Path: {}".format(self._file_path) ) # Making it an Action Client self._ac = actionlib.ActionClient('/action_ros_iot', msgRosIotAction) # Dictionary to store the goal handles self._goal_handles = {} # Wait for the action server to start rospy.loginfo("[UR5_1] Waiting for Action Server ...") self._ac.wait_for_server() rospy.loginfo('\033[94m' + "[UR5_1]: >>> Ur5Moveit init done." + '\033[0m')
def shallow_insertion_start(): global gripper_command_publisher ## 1. Initialize robot environment print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. scene = moveit_commander.PlanningSceneInterface() ## Instantiate a MoveGroupCommander object. This object is an interface ## to one group of joints. In this case the group is the joints in the left ## arm. This interface can be used to plan and execute motions on the left ## arm. ##group = moveit_commander.MoveGroupCommander("left_arm") group = moveit_commander.MoveGroupCommander("manipulator") #set max velocity and acceleration scaler group.set_max_velocity_scaling_factor(0.1); group.set_max_acceleration_scaling_factor(0.1); ## We create this DisplayTrajectory publisher which is used below to publish ## trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) #initialize the gripper publisher variable gripper_command_publisher = rospy.Publisher( 'CModelRobotInputRob', String, queue_size=20) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(2) print "============ Starting tutorial " ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ ## ## We can get the name of the reference frame for this robot print "============ Reference frame: %s" % group.get_planning_frame() ## We can also print the name of the end-effector link for this group print "============ End effector: %s" % group.get_end_effector_link() ## We can get a list of all the groups in the robot print "============ Robot Groups:" print robot.get_group_names() ## Sometimes for debugging it is useful to print the entire state of the ## robot. print "============ Printing group state" print group.get_current_pose() # ## Then, we will get the current set of joint values for the group # group_variable_values = group.get_current_joint_values() # print "============ Joint values: ", group_variable_values ## Now, let's modify one of the joints, plan to the new joint ## space goal and visualize the plan group_variable_values = [-0.02749187151064092, -1.598161522542135, -1.8247435728656214, -3.7606776396380823, -1.3931792418109339, 1.796911358833313] group.set_joint_value_target(group_variable_values) plan2 = group.plan() print "============ Waiting while RVIZ displays plan2..." group.execute(plan2) rospy.sleep(2) # moveit_commander.roscpp_shutdown() # return # Initialize robot environment END #2. Check if the gripper works well, then open it gripper_init() waypoints = [] #3. Go to the beginning position before closing the gripper and pick up the card pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = 0.5 pose_target.orientation.y = 0.5 pose_target.orientation.z = -0.5 pose_target.orientation.w = 0.5 pose_target.position.x = 0.02 -0.805 pose_target.position.y = 0.235 - 0.0125 pose_target.position.z = -0.12 + 0.46 - 0.03 group.set_pose_target(pose_target) plan1 = group.plan() group.execute(plan1) rospy.sleep(2) #4. Close the gripper gripper_close() #5. Pick up the card, and move to the top of the beginning point pose_target.position.z = -0.12 + 0.46 + 0.1 group.set_pose_target(pose_target) plan1 = group.plan() group.execute(plan1) rospy.sleep(2) #6. Move to the top of the insertion beginning point pose_target.position.x = 0.02 -0.805 pose_target.position.y = 0.235 - 0.0125 - 0.225 - 0.002 pose_target.position.z = -0.12 + 0.1 + 0.46 group.set_pose_target(pose_target) plan1 = group.plan() group.execute(plan1) rospy.sleep(2) ##7. Go down and start the insertion process pose_target.orientation.x = 0.48868426425 pose_target.orientation.y = 0.504593823773 pose_target.orientation.z = -0.51414136297 pose_target.orientation.w = 0.492170114665 pose_target.position.x = -0.77998840649 pose_target.position.y = 0.00084252595 pose_target.position.z = 0.369628519 group.set_pose_target(pose_target) plan1 = group.plan() group.execute(plan1) print 'point 1' rospy.sleep(2) #calculate ARC waypoints center_point = [-0.77998840649, 0.00084252595, 0.369628519 - 0.315] rotate_axis = [0, 1, 0] waypoints_new = calc_waypoints_ARC(pose_target, center_point, rotate_axis, 45) #execute path with waypoints ## We want the cartesian path to be interpolated at a resolution of 1 cm ## which is why we will specify 0.01 as the eef_step in cartesian ## translation. We will specify the jump threshold as 0.0, effectively ## disabling it. (plan3, fraction) = group.compute_cartesian_path( waypoints_new, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold group.execute(plan3) rospy.sleep(2) #13. Widen the gripper print 'gripper 200' gripper_200() #14. Move to the 8th way point in the insertion process pose_target.orientation.x = 0.300275507697 pose_target.orientation.y = 0.622241259232 pose_target.orientation.z = -0.638282291773 pose_target.orientation.w = 0.339479234704 pose_target.position.x = -0.586538559597 pose_target.position.y = 0.010062837864 pose_target.position.z = 0.301186931067 group.set_pose_target(pose_target) plan1 = group.plan() group.execute(plan1) print 'point 8' rospy.sleep(2) #15. Widen the gripper gripper_open_degree('180') #16. Move to the 9th way point in the insertion process pose_target.orientation.x = 0.347952330633 pose_target.orientation.y = 0.63268202582 pose_target.orientation.z = -0.596866944095 pose_target.orientation.w = 0.349846367507 pose_target.position.x = -0.597923336817 pose_target.position.y = -0.00229020222052 pose_target.position.z = 0.305400357222 group.set_pose_target(pose_target) plan1 = group.plan() group.execute(plan1) print 'point 9' rospy.sleep(2) #17. Move to the 10th way point in the insertion process pose_target.orientation.x = 0.359182117133 pose_target.orientation.y = 0.630010602994 pose_target.orientation.z = -0.585982817717 pose_target.orientation.w = 0.361523144746 pose_target.position.x = -0.594702188085 pose_target.position.y = -0.00658854236333 pose_target.position.z = 0.31898880102 group.set_pose_target(pose_target) plan1 = group.plan() group.execute(plan1) print 'point 10' rospy.sleep(2) #18. Move to the 11th way point in the insertion process pose_target.orientation.x = 0.364964183344 pose_target.orientation.y = 0.628658362491 pose_target.orientation.z = -0.580710320412 pose_target.orientation.w = 0.366558770068 pose_target.position.x = -0.592416763802 pose_target.position.y = -0.00599166509442 pose_target.position.z = 0.320244273086 group.set_pose_target(pose_target) plan1 = group.plan() group.execute(plan1) print 'point 11' rospy.sleep(2) #19. Move to the 12th way point in the insertion process pose_target.orientation.x = 0.396925730678 pose_target.orientation.y = 0.583354341151 pose_target.orientation.z = -0.680227755847 pose_target.orientation.w = 0.198589720686 pose_target.position.x = -0.527264025781 pose_target.position.y = 0.194388091811 pose_target.position.z = 0.437013772921 group.set_pose_target(pose_target) plan1 = group.plan() group.execute(plan1) print 'point 12' rospy.sleep(2) # The card should have fallen into the shallow hole print "============ STOPPING" # Shutdown moveit moveit_commander.roscpp_shutdown() return
def __init__(self): ############ # new rclpy.init() ############ super(MoveGroupPythonIntefaceTutorial, self).__init__() ####################################################### # new # rclpyでのQoSを指定する方法 self.qos_profile = QoSProfile(depth=10) ####################################################### ## BEGIN_SUB_TUTORIAL setup ## ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) ####################################################################### # rclpy.nodeを使用することで、rclpy.init()の後はselfでnodeの代わりになる # old # rospy.init_node('move_group_python_interface_tutorial', anonymous=True) ######################################################################## ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's ## kinematic model and the robot's current joint states robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface ## for getting, setting, and updating the robot's internal understanding of the ## surrounding world: scene = moveit_commander.PlanningSceneInterface() ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to a planning group (group of joints). In this tutorial the group is the primary ## arm joints in the Panda robot, so we set the group's name to "panda_arm". ## If you are using a different robot, change this value to the name of your robot ## arm planning group. ## This interface can be used to plan and execute motions: group_name = "panda_arm" move_group = moveit_commander.MoveGroupCommander(group_name) ## Create a `DisplayTrajectory`_ ROS publisher which is used to display ## trajectories in Rviz: #################################################################################### # old # display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', # moveit_msgs.msg.DisplayTrajectory, # queue_size=20) #################################################################################### # new # 参考:https://docs.ros2.org/crystal/api/rclpy/api/node.html # https://docs.ros.org/en/foxy/Contributing/Migration-Guide-Python.html#creating-a-publisher # rospyとの違い:topicとmsg_typeが逆 display_trajectory_publisher = self.create_subscription( moveit_msgs.msg.DisplayTrajectory, '/move_group/display_planned_path', self.qos_profile) #################################################################################### ## END_SUB_TUTORIAL ## BEGIN_SUB_TUTORIAL basic_info ## ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ # 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("") ## END_SUB_TUTORIAL # Misc variables self.box_name = '' self.robot = robot self.scene = scene self.move_group = move_group self.display_trajectory_publisher = display_trajectory_publisher self.planning_frame = planning_frame self.eef_link = eef_link self.group_names = group_names
def __init__(self): super(UR5PickupPlace, self).__init__() ## BEGIN_SUB_TUTORIAL setup ## ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('UR5_pickup_place', anonymous=True) ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's ## kinematic model and the robot's current joint states robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface ## for getting, setting, and updating the robot's internal understanding of the ## surrounding world: scene = moveit_commander.PlanningSceneInterface() ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to a planning group (group of joints). In this tutorial the group is the primary ## arm joints in the Panda robot, so we set the group's name to "panda_arm". ## If you are using a different robot, change this value to the name of your robot ## arm planning group. ## This interface can be used to plan and execute motions: group_name = "manipulator" move_group_arm = moveit_commander.MoveGroupCommander(group_name) move_group_arm.set_planning_time(20) move_group_arm.allow_looking(True) move_group_arm.set_num_planning_attempts(8) group_name = "gripper" move_group_gripper = moveit_commander.MoveGroupCommander(group_name) move_group_gripper.set_planning_time(20) move_group_gripper.allow_looking(True) move_group_gripper.set_num_planning_attempts(8) # Gazebo reset self.pub_gazebo_object = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=1) self.pub_running_state = rospy.Publisher('running_state', String, queue_size=1) ## BEGIN_SUB_TUTORIAL basic_info ## ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ # We can get the name of the reference frame for this robot: planning_frame = move_group_arm.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_arm.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 "" ## END_SUB_TUTORIAL self.box_name = '' self.robot = robot self.scene = scene self.move_group_arm = move_group_arm self.move_group_gripper = move_group_gripper self.planning_frame = planning_frame self.eef_link = eef_link self.group_names = group_names # Record the initial position of the object object = rospy.wait_for_message('gazebo/model_states', ModelStates) for i in range(len(object.name)): if object.name[i] == 'object': object_reset = ModelState() object_reset.model_name = object.name[i] object_reset.pose = object.pose[i] self.object_initial_position = object_reset
def tilt_pivot_start(): global gripper_command_publisher global odom_pub global odom_c_pub odom_pub = rospy.Publisher("/odom", Odometry, queue_size=50) # diaplay center point odom_c_pub = rospy.Publisher("/odom_c", Odometry, queue_size=50) global display_trajectory_trigger_pub display_trajectory_trigger_pub = rospy.Publisher( '/display_trigger', String, queue_size=20) moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) robot = moveit_commander.RobotCommander() global group group = moveit_commander.MoveGroupCommander("manipulator") #set max velocity and acceleration scaler group.set_max_velocity_scaling_factor(0.5); group.set_max_acceleration_scaling_factor(0.5); group.set_planning_time(3) #initialize the gripper publisher variable gripper_command_publisher = rospy.Publisher( 'CModelRobotInputRob', String, queue_size=20) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(2) print "============ Starting tutorial " for phi in range(0, 91, 10): for alpha in range(0, -91, -10): for beta in range(0, 91, 10): for gamma in range(0, -91, -10): card_length = 0.17 #go to camera point clean_scene() group_variable_values = [-55.77/ 180 * 3.1415926, -95.61/ 180 * 3.1415926, -72.13/ 180 * 3.1415926, 77.75/ 180 * 3.1415926, -89.71/ 180 * 3.1415926, 124.15/ 180 * 3.1415926] group.set_joint_value_target(group_variable_values) plan1 = group.plan() group.execute(plan1) # print 'go to the start point of the ARC' rospy.sleep(2) center_point = [-0.77998840649 + 0.5, 0.00084252595, 0.369628519 - 0.315] rotate_axis = [0, 0, 1] # Set the rotation axis as 'z axis' set_tool(-0.073, 0.005, 0.215) f = open('/home/sslrayray/output.txt','a') group_move_tool('z', alpha) group_move_tool('y', 8) pose_target = group.get_current_pose().pose pose_target.position.y += 0.3 pose_target.position.z = 0.219 #0.269 group.set_pose_target(pose_target) plan1 = group.plan() group.execute(plan1) tool_pose = get_tool_position() external_axis_center = tool_pose external_axis_center[1] += card_length # print 'tilt center', external_axis_center group_rotate_by_external_axis(external_axis_center, [-1, 0, 0], phi) group_move_tool('y', beta) group_move_tool('x', gamma) final_tool_center = get_tool_position() # print 'final tool center', final_tool_center card_center = [(external_axis_center[0] + final_tool_center[0])/2, (external_axis_center[1] + final_tool_center[1])/2, (external_axis_center[2] + final_tool_center[2])/2 ] # print 'card center =', card_center # print 'angle = ', phi / 180.0 * 3.1415926 card_pose = [final_tool_center[0] - 0.07, final_tool_center[1] - 0.001, final_tool_center[2]+0.003, 3.1415926 - phi / 180.0 * 3.1415926, 3.1415926, -3.1415926] add_planning_scene(card_pose) res = check_collision() if res: print 'Phi:',phi, ', Alpha:',alpha, ', Beta:', beta, ', Gamma:', gamma, ', Collision Result: Yes' string_val = str(phi) + ',' + str(alpha) + ',' + str(beta) + ',' + str(gamma) + ',' + 'Y\n' else: print 'Phi:',phi, ', Alpha:',alpha, ', Beta:', beta, ', Gamma:', gamma, ', Collision Result: No' string_val = str(phi) + ',' + str(alpha) + ',' + str(beta) + ',' + str(gamma) + ',' + 'N\n' f.write(string_val) f.close() raw_input() rospy.sleep(3) # Shutdown the program moveit_commander.roscpp_shutdown() print 'STOPPING'
def body_up(): # 取りに行く robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") arm.set_max_velocity_scaling_factor(1) gripper = moveit_commander.MoveGroupCommander("gripper") arm.set_named_target("home") arm.go() # ハンドを閉じる gripper.set_joint_value_target([0.8, 0.8]) gripper.go() target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.205045831868 target_pose.position.y = -0.0194597655966 target_pose.position.z = 0.309807278119 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = 0.474871942335 target_pose.orientation.y = 0.424353826113 target_pose.orientation.z = 0.514882237833 target_pose.orientation.w = 0.573861263557 arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行take target_pose = geometry_msgs.msg.Pose() target_pose.position.x = 0.269119370689 target_pose.position.y = -0.0439712214052 target_pose.position.z = 0.138551614587 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = 0.717972912926 target_pose.orientation.y = 0.671395173952 target_pose.orientation.z = 0.10624984559 target_pose.orientation.w = 0.149847879569 arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行take # ハンドを閉じる gripper.set_joint_value_target([0.1, 0.1]) gripper.go() arm.set_named_target("home") arm.go() # 物体を持ち上げる target_pose = geometry_msgs.msg.Pose() target_pose.position.x = -0.0372058227489 target_pose.position.y = -0.246006903877 target_pose.position.z = 0.169424000349 q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = -0.475279946015 target_pose.orientation.y = 0.431250538961 target_pose.orientation.z = -0.753061012631 target_pose.orientation.w = 0.145020884072 arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 gripper.set_joint_value_target([0.7, 0.7]) gripper.go() rospy.sleep(2.0) # ハンドを閉じる gripper.set_joint_value_target([0.1, 0.1]) gripper.go() arm.set_named_target("home") arm.go() print("done")
def main(): # Inicializo nodo - haptic_jointpose - rospy.init_node('haptic_jointpose', anonymous=False) # Moveit commander, robot commander y planning scene moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() # Subscribo topics /Geomagic/button y /Geomagic/joint_status sub_botones = rospy.Subscriber("/Geomagic/button", DeviceButtonEvent, Callback_botones) sub_joints = rospy.Subscriber("/Geomagic/joint_states", JointState, Callback_joints) # Subscribo topic fuerzas /wrench sub_force = rospy.Subscriber("/wrench", WrenchStamped, Callback_force) # Publish force feedback pub = rospy.Publisher('/Geomagic/force_feedback', DeviceFeedback, queue_size=1) #r = rospy.Rate(10) # Instancio MoveGroupCommander, para UR5e - manipulator - (Robot planning group) move_group = moveit_commander.MoveGroupCommander("manipulator") # Publico topic /move_group/display_planned_path display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # Inicializo joint vars joint_goal = move_group.get_current_joint_values() joint_haptic = [0, 0, 0, 0, 0, 0] joint_goal = [ pi / 2, -0.2689 * 2, 0.6397 + pi / 2, -pi + pi / 5, -pi / 2 + pi, pi ] df = DeviceFeedback() ############################ # Main while rospy running # ############################ iii = 1 while not rospy.is_shutdown(): ###################### # Push button action # ###################### if (boton_gris.data == 1): joint_haptic = [ waist.data, shoulder.data, elbow.data, yaw.data, pitch.data, roll.data ] print joint_haptic #r.sleep() joint_goal[0] = joint_goal[0] joint_goal[1] = joint_goal[1] - 0.05 joint_goal[2] = joint_goal[2] joint_goal[3] = joint_goal[3] joint_goal[4] = joint_goal[4] joint_goal[5] = joint_goal[5] ##### Final if grey button ##### # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group move_group.go(joint_goal, wait=True)
def __init__(self): super(UR5GripperPythonInteface, self).__init__() ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('ur5_gripper_python_interface', anonymous=True) ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's ## kinematic model and the robot's current joint states robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface ## for getting, setting, and updating the robot's internal understanding of the ## surrounding world: scene = moveit_commander.PlanningSceneInterface() ## Create a `DisplayTrajectory`_ ROS publisher which is used to display ## trajectories in Rviz: display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # 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 "" ## END_SUB_TUTORIAL # Misc variables self.box_name = '' self.robot = robot self.scene = scene self.display_trajectory_publisher = display_trajectory_publisher self.group_names = group_names init=False def initialize_groups(): self.arm_group = self.robot.get_group("ur5_arm") self.gripper_group = self.robot.get_group("robotiq140gripper") return start_time = time.time() while(init == False): try: initialize_groups() init = True except: time_now = time.time() #try for 120 seconds if(time_now - start_time > 120): print "Could not initialize groups ..." print "shutdown" sys.exit(1) print "Failed to initialize groups, trying again ..."
def __init__(self): # Initialize the move_group API and node moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_ik_demo', anonymous=True) robot = moveit_commander.RobotCommander()
def move_group_python_interface_tutorial(): ## BEGIN_TUTORIAL ## ## Setup ## ^^^^^ ## CALL_SUB_TUTORIAL imports ## ## First initialize moveit_commander and rospy. print "============ Starting tutorial setup" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) ## Instantiate a RobotCommander object. This object is an interface to ## the robot as a whole. robot = moveit_commander.RobotCommander() ## Instantiate a PlanningSceneInterface object. This object is an interface ## to the world surrounding the robot. scene = moveit_commander.PlanningSceneInterface() ## Instantiate a MoveGroupCommander object. This object is an interface ## to one group of joints. In this case the group is the joints in the left ## arm. This interface can be used to plan and execute motions on the left ## arm. group = moveit_commander.MoveGroupCommander("manipulator") ## We create this DisplayTrajectory publisher which is used below to publish ## trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) ## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up. print "============ Waiting for RVIZ..." rospy.sleep(1) print "============ Starting tutorial " ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ ## ## We can get the name of the reference frame for this robot print "============ Reference frame: %s" % group.get_planning_frame() ## We can also print the name of the end-effector link for this group print "============ Reference frame: %s" % group.get_end_effector_link() ## We can get a list of all the groups in the robot print "============ Robot Groups:" print 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 "============" ## Planning to a Pose goal ## ^^^^^^^^^^^^^^^^^^^^^^^ ## We can plan a motion for this group to a desired pose for the ## end-effector print "============ Generating plan 1" # Here I am making it go to some Pose (recall Pose has coordinates(x,y,z) # as well as orientation (quaternions) ) # My robot is in the up position to start and this Pose will make its # wrist dip down by 0.3 meters. 1.0-0.2=0.8 meters i.e. position.z pose_target = geometry_msgs.msg.Pose() quaternion = tf.transformations.quaternion_from_euler(0, 0, 0) pose_target.orientation.x = quaternion[0] pose_target.orientation.y = quaternion[1] pose_target.orientation.z = quaternion[2] pose_target.orientation.w = quaternion[3] pose_target.position.x = 0 pose_target.position.y = 0.2 pose_target.position.z = 0.8 #tell ur5 that you want it to go to the Pose group.set_pose_target(pose_target) #if you don't have this line, the robot will go too fast and emergency stop group.set_max_velocity_scaling_factor(0.2) ## Now, we call the planner to compute the plan ## and visualize it if successful ## Note that we are just planning, not asking move_group ## to actually move the robot plan1 = group.plan() print "============ Waiting while RVIZ displays plan1..." rospy.sleep(0.5) ## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the ## group.plan() method does this automatically so this is not that useful ## here (it just displays the same trajectory again). print "============ Visualizing plan1" display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan1) display_trajectory_publisher.publish(display_trajectory) print "============ Waiting while plan1 is visualized (again)..." rospy.sleep(0.5) ## Moving to a pose goal ## ^^^^^^^^^^^^^^^^^^^^^ ## ## Moving to a pose goal is similar to the step above ## except we now use the go() function. Note that ## the pose goal we had set earlier is still active ## and so the robot will try to move to that goal. We will ## not use that function in this tutorial since it is ## a blocking function and requires a controller to be active ## and report success on execution of a trajectory. # Uncomment below line when working with a real robot print "Press y to move robot to goal state. Press any other key to exit." if raw_input() == 'y': group.go(wait=True) """ ## Planning to a joint-space goal ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## ## Let's set a joint space goal and move towards it. ## First, we will clear the pose target we had just set. group.clear_pose_targets() ## Then, we will get the current set of joint values for the group group_variable_values = group.get_current_joint_values() print "============ Joint values: ", group_variable_values ## Now, let's modify one of the joints, plan to the new joint ## space goal and visualize the plan group_variable_values[0] = 1.0 group.set_joint_value_target(group_variable_values) plan2 = group.plan() print "============ Waiting while RVIZ displays plan2..." rospy.sleep(5) ## Cartesian Paths ## ^^^^^^^^^^^^^^^ ## You can plan a cartesian path directly by specifying a list of waypoints ## for the end-effector to go through. waypoints = [] # start with the current pose waypoints.append(group.get_current_pose().pose) # first orient gripper and move forward (+x) wpose = geometry_msgs.msg.Pose() wpose.orientation.w = 1.0 wpose.position.x = waypoints[0].position.x + 0.1 wpose.position.y = waypoints[0].position.y wpose.position.z = waypoints[0].position.z waypoints.append(copy.deepcopy(wpose)) # second move down wpose.position.z -= 0.10 waypoints.append(copy.deepcopy(wpose)) # third move to the side wpose.position.y += 0.05 waypoints.append(copy.deepcopy(wpose)) ## We want the cartesian path to be interpolated at a resolution of 1 cm ## which is why we will specify 0.01 as the eef_step in cartesian ## translation. We will specify the jump threshold as 0.0, effectively ## disabling it. (plan3, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print "============ Waiting while RVIZ displays plan3..." rospy.sleep(5) ## Adding/Removing Objects and Attaching/Detaching Objects ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## First, we will define the collision object message collision_object = moveit_msgs.msg.CollisionObject() ## When finished shut down moveit_commander. moveit_commander.roscpp_shutdown() """ ## END_TUTORIAL print "============ STOPPING==== Program will end now"
def go_target(): global cxm global cym global pub #Initialize node rospy.init_node('pick_place', anonymous=True) rospy.Subscriber("/arbotix/force", Analog, callback_force) rospy.Subscriber("/gpg/camera_info", CameraInfo, callback_camera) pub = rospy.Publisher("/gripper_joint/command", Float64, queue_size=5) tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) direction = PointStamped() direction.header.stamp = rospy.Time() direction.header.frame_id = "camera_link" robot = moveit_commander.RobotCommander() group = moveit_commander.MoveGroupCommander("arm") group.set_goal_tolerance(0.01) group.set_planner_id('RRTstarkConfigDefault') group.set_planning_time(0.5) r = rospy.Rate(1) time.sleep(1) pub.publish(0) time.sleep(1) while not rospy.is_shutdown(): # Go to initial position go_to_pinitial(group) #Wait for user input obj = raw_input("Write the object that you want to pick up or write 'quit' to exit: ") if obj == 'quit': break print obj #Read center of mass f= open("/home/nvidia/catkin_ws/src/project/src/object.fcf","w") f.write(obj) f.close() f = open("data.fcf",'r') args = f.readline() args = args.split(',') args = numpy.array(args, dtype=numpy.float64) #Calculate object position cxm = int((args[1] + args[3])*0.5) cym = int(args[2]) position = None line = model.projectPixelTo3dRay((cxm,cym)) if cxm == 0 and cym == 0: print "zero" pass direction.point.x = line[0] direction.point.y = line[1] direction.point.z = line[2] try: q = tfBuffer.transform(direction, "base_link") direction.point.x = 0 direction.point.y = 0 direction.point.z = 0 p = tfBuffer.transform(direction, "base_link") position = compute_position(p,q) pose_target.position.x = position[0] + 0.01 pose_target.position.y = position[1] if obj == 'bottle': pose_target.position.z = position[2] + 0.065 else: pose_target.position.z = position[2] + 0.03 if pose_target.position.z < -0.052: pose_target.position.z = -0.052 angle = math.atan2(pose_target.position.y,pose_target.position.x) quart = tf.transformations.quaternion_from_euler(0, 1.5707, angle) pose_target.orientation.x = quart[0] pose_target.orientation.y = quart[1] pose_target.orientation.z = quart[2] pose_target.orientation.w = quart[3] except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: print e r.sleep() # Send the coordinates to Rviz broadcaster = tf2_ros.TransformBroadcaster() t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "base_link" t.child_frame_id = "object" t.transform.translation.x = position[0] t.transform.translation.y = position[1] t.transform.translation.z = position[2] t.transform.rotation.x = 0 t.transform.rotation.y = 0 t.transform.rotation.z = 0 t.transform.rotation.w = 1 broadcaster.sendTransform(t) # Publish the object position print "pose_target" print pose_target group.set_pose_target(pose_target) # Pick up the object and come back to initial position group.go(wait=True) time.sleep(1) get_object() go_to_pinitial(group) go_to_pfinal(group) pub.publish(0) time.sleep(0.5) go_to_pinitial(group)
def send_move_command(self): #We want orientation to always be the same bMove = False #stopped if self.bstop: rospy.sleep(1) bMove = False #move to basket elif self.btoBasket: position = self.moveMethod[1](self) bMove = True #move to last cut location elif self.btoCut: position = self.moveMethod[2](self) bMove = True #move by camera elif self.bbyCamera: position = self.moveMethod[3](self) bMove = True #move by scan elif self.bbyScan: position = self.moveMethod[4](self) bMove = True if bMove: print position if self.bgripper: #make new message goal = kinova_msgs.msg.SetFingersPositionGoal() goal.fingers.finger1 = self.finger_turn goal.fingers.finger2 = goal.fingers.finger1 goal.fingers.finger3 = goal.fingers.finger1 print goal #send through client self.fingerClient.send_goal(goal) print "Attention: moving the gripper" if self.fingerClient.wait_for_result(rospy.Duration(5.0)): print 'Success' else: self.fingerClient.cancel_all_goals() print ' Error: the cartesian action timed-out' self.bgripper = False if self.finger_turn == finger_minTurn: #if we just opened fingers return to last cut location self.btoBasket = False self.btoCut = True self.finger_turn = finger_maxTurn else: #if we just closed to fngers move to basket self.finger_turn = finger_minTurn self.btoBasket = True else: #basket has a different orientation orientation = EulerXYZ2Quaternion(self.currentRPY) if self.btoBasket: orientation = EulerXYZ2Quaternion(self.basketRPY) #send with Moveit using location gotten above to move to the new location goal = geometry_msgs.msg.Pose() goal.position = geometry_msgs.msg.Point(x=position[0], y=position[1], z=position[2]) goal.orientation = geometry_msgs.msg.Quaternion( x=orientation[0], y=orientation[1], z=orientation[2], w=orientation[3]) # Moveit robot = moveit_commander.RobotCommander() group = moveit_commander.MoveGroupCommander("arm") planning_scene_interface = moveit_commander.PlanningSceneInterface( ) # Planning to a Pose goal group.set_pose_target(goal) # Now, we call the planner to compute the plan and visualize it. group.plan() # we should add something hear to handle failures # move the robot print "Attention: moving the arm" group.go()
def Test(): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('comportement_controller', anonymous=True) # create a publisher say = rospy.Publisher('/qt_robot/speech/say', String, queue_size=10) emo = rospy.Publisher('/qt_robot/emotion/show',String, queue_size=15) head_pub = rospy.Publisher('/qt_robot/head_position/command',Float64MultiArray, queue_size=1) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() right_arm = moveit_commander.MoveGroupCommander("right_arm") left_arm = moveit_commander.MoveGroupCommander("left_arm") # head = moveit_commander.MoveGroupCommander("head") rospy.sleep(3) # We can get the name of the reference frame for this robot: planning_frame_left = left_arm.get_planning_frame() planning_frame_right= right_arm.get_planning_frame() print "============Left reference frame: %s " % planning_frame_left print "============Right reference frame: %s " %planning_frame_right # We can also print the name of the end-effector link for this group: eef_link_left = left_arm.get_end_effector_link() eef_link_right = right_arm.get_end_effector_link() print "============Left end effector: %s " % eef_link_left print "============Right end effector: %s " % eef_link_right # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print "============ Robot Groups:", robot.get_group_names() left_arm.allow_replanning(True) left_arm.set_pose_reference_frame("base_link") left_arm.set_planning_time(3.0) left_arm.clear_path_constraints() left_arm.clear_pose_targets() right_arm.allow_replanning(True) right_arm.set_pose_reference_frame("base_link") right_arm.set_planning_time(3.0) right_arm.clear_path_constraints() right_arm.clear_pose_targets() # Sometimes for debugging it is useful to print the entire state of the # robot: print "============ Printing robot state" print robot.get_current_state() print "" ## END_SUB_TUTORIAL print "============ Generating plan 1" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.w = 1.0 pose_target.position.x = 0.7 pose_target.position.y = -0.05 pose_target.position.z = 1.1 group.set_pose_target(pose_target) ## to actually move the robot plan1 = left_arm.plan() print "============ Waiting while RVIZ displays plan1..." rospy.sleep(5)
def __init__(self): super(MoveGroupPythonIntefaceTutorial, self).__init__() ## BEGIN_SUB_TUTORIAL setup ## ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface_tutorial', anonymous=True) ## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to ## the robot: robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This object is an interface ## to the world surrounding the robot: scene = moveit_commander.PlanningSceneInterface() ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to one group of joints. In this case the group is the joints in the Panda ## arm so we set ``group_name = panda_arm``. If you are using a different robot, ## you should change this value to the name of your robot arm planning group. ## This interface can be used to plan and execute motions on the Panda: group_name = "manipulator" group = moveit_commander.MoveGroupCommander(group_name) ## We create a `DisplayTrajectory`_ publisher which is used later to publish ## trajectories for RViz to visualize: display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) ## END_SUB_TUTORIAL ## BEGIN_SUB_TUTORIAL basic_info ## ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ # We can get the name of the reference frame for this robot: planning_frame = group.get_planning_frame() print "============ Reference frame: %s" % planning_frame # We can also print the name of the end-effector link for this group: eef_link = group.get_end_effector_link() print "============ End effector: %s" % eef_link # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print "============ Robot 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 "" ## END_SUB_TUTORIAL # Misc variables self.box_name = '' self.robot = robot self.scene = scene self.group = group self.display_trajectory_publisher = display_trajectory_publisher self.planning_frame = planning_frame self.eef_link = eef_link self.group_names = group_names
def main(): rospy.init_node("crane_x7_pick_and_place_controller") robot = moveit_commander.RobotCommander() arm = moveit_commander.MoveGroupCommander("arm") #アームの動きの速さを示している arm.set_max_velocity_scaling_factor(0.5) gripper = moveit_commander.MoveGroupCommander("gripper") while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0: rospy.sleep(1.0) rospy.sleep(1.0) print("Group names:") print(robot.get_group_names()) print("Current state:") print(robot.get_current_state()) # アーム初期ポーズを表示 arm_initial_pose = arm.get_current_pose().pose print("Arm initial pose:") print(arm_initial_pose) # SRDFに定義されている"home"の姿勢にする arm.set_named_target("home") arm.go() gripper.set_joint_value_target([0.7, 0.7]) gripper.go() #繰り返し呼び出すのでmove関数を定義する def move(x, y, z): target_pose = geometry_msgs.msg.Pose() target_pose.position.x = x target_pose.position.y = y target_pose.position.z = z q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合 target_pose.orientation.x = q[0] target_pose.orientation.y = q[1] target_pose.orientation.z = q[2] target_pose.orientation.w = q[3] arm.set_pose_target(target_pose) # 目標ポーズ設定 arm.go() # 実行 # 掴む準備をする move(0.2, 0.0, 0.2) # 掴みに行く move(0.2, 0.0, 0.1) # 持ち上げる move(0.2, 0.0, 0.2) # 移動する move(0.2, 0.3, 0.2) # 下ろす move(0.2, 0.3, 0.1) # 少しだけハンドを持ち上げる move(0.2, 0.3, 0.15) # SRDFに定義されている"home"の姿勢にする # srdfファイルがある場所 (crane_x7_ros/crane_x7_moveit_config/config/crane_x7.srdf) arm.set_named_target("home") arm.go() print("done")
def handle_req(req): ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) ## Instantiate a `RobotCommander`_ object. Provides information such as the robot's ## kinematic model and the robot's current pose states robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface ## for getting, setting, and updating the robot's internal understanding of the ## surrounding world: scene = moveit_commander.PlanningSceneInterface() ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to a planning group (group of poses). ## This interface can be used to plan and execute motions: group_name = req.group ## print "============ Robot Groups:" print robot.get_group_names() ## print "============ Printing robot state" print robot.get_current_state() move_group = moveit_commander.MoveGroupCommander(group_name.data) print "Pose is: " print move_group.get_current_pose().pose print "Orientation is: " print req.orientation print "x is: " print req.posex print "y is: " print req.posey print "z is: " print req.posez ## Create a `DisplayTrajectory`_ ROS publisher which is used to display ## trajectories in Rviz: display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) # We can get the pose values from the group and adjust some of the values: print "============ Generating plan.." pose_goal = geometry_msgs.msg.Pose() print pose_goal # plan a motion for this group pose_goal.orientation.w = req.orientation.data pose_goal.position.x = req.posex.data pose_goal.position.y = req.posey.data pose_goal.position.z = req.posez.data move_group.set_pose_target(pose_goal) # The go command can be called with pose values, poses, or without any # parameters if you have already set the pose or pose target for the group move_group.go(pose_goal, wait=True) # Calling ``stop()`` ensures that there is no residual movement move_group.stop() # It is always good to clear your targets after planning with poses. move_group.clear_pose_targets() rep = udm_poseService() rep.message = "This action was planned at %s" % rospy.get_time() rep.res = True return rep
def main(): arg_fmt = argparse.RawDescriptionHelpFormatter parser = argparse.ArgumentParser(formatter_class=arg_fmt, description=main.__doc__) required = parser.add_argument_group('required arguments') required.add_argument('-l', '--limb', required=True, choices=['left', 'right'], help='send joint trajectory to which limb') args = parser.parse_args(rospy.myargv()[1:]) limb = args.limb rospy.init_node('stackit') iksvc, ns = ik_command.connect_service(limb) moveit_commander.roscpp_initialize(sys.argv) rate = rospy.Rate(1) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander(limb + "_arm") group.allow_replanning(True) print("Getting robot state... ") rs = baxter_interface.RobotEnable(CHECK_VERSION) print("Enabling robot... ") rs.enable() #Calibrate gripper gripper_if = baxter_interface.Gripper(limb) if not gripper_if.calibrated(): print "Calibrating gripper" gripper_if.calibrate() obj_manager = ObjectManager() while len(obj_manager.collision_objects) <= 0: rate.sleep() objects = obj_manager.collision_objects object_height = params['object_height'] """if len(objects) > 1: stack_pose = projectPose(objects[len(objects)-1].primitive_poses[0]) stack_pose = incrementPoseMsgZ(stack_pose, object_height*2.0) objects.pop(len(objects)-1) elif len(objects) == 1: stack_pose = projectPose(objects[0].primitive_poses[0])""" stack_pose = Pose(position=Point(0.593, -0.212, object_height - 0.130), orientation=Quaternion(0.6509160466, 0.758886809948, -0.0180992582839, -0.0084573527776)) processed_win = "Processed image" raw_win = "Hand camera" cv2.namedWindow(processed_win) #cv2.namedWindow(raw_win) rospy.on_shutdown(obj_manager.remove_known_objects) for obj in objects: obj_pose = obj.primitive_poses[0] #group.pick(obj.id) #group.place(obj.id, stack_pose) print "Got pose:", obj.primitive_poses[0] pose = projectPose(obj.primitive_poses[0]) pose = incrementPoseMsgZ(pose, object_height * 1.7) print "Modified pose:", pose #if obj.id in obj_manager.id_operations: # obj_manager.id_operations[obj.id] = CollisionObject.REMOVE #obj_manager.publish(obj) print "setting target to pose" # Move to the next block group.clear_pose_targets() group.set_start_state_to_current_state() group.set_pose_target(pose) plan = group.plan() # is there a better way of checking this? plan_found = len(plan.joint_trajectory.points) > 0 if plan_found: #print "============ Waiting while RVIZ displays plan1..." #rospy.sleep(3) group.go(wait=True) imgproc = ObjectFinder("star", None, None) imgproc.subscribe("/cameras/" + limb + "_hand_camera/image") imgproc.publish(limb) vc = VisualCommand(iksvc, limb) vc.subscribe() while (not vc.done) and (not rospy.is_shutdown()): blobArray = [] for centroid, axis in zip(imgproc.centroids, imgproc.axes): blob = BlobInfo() centroid = Point(*centroid) blob.centroid = centroid if axis is None: axis = -1 * numpy.ones(6) blob.axis = Polygon([ Point(*axis[0:3].tolist()), Point(*axis[3:6].tolist()) ]) blobArray.append(blob) msg = BlobInfoArray() msg.blobs = blobArray imgproc.handler_pub.publish(msg) if imgproc.processed is not None: cv2.imshow(processed_win, imgproc.processed) cv2.waitKey(10) vc.unsubscribe() imgproc.unsubscribe() print "Adding attached message" #Add attached message #obj.primitive_poses[0] = incrementPoseMsgZ(obj.primitive_poses[0], *object_height) #this is in the base frame...? obj_manager.publish_attached(obj, limb) #touch_links = [limb+"_gripper", limb+"_gripper_base", limb+"_hand_camera", limb+"_hand_range"] #group.attach_object(obj.id, limb+"_gripper", touch_links = touch_links) # Carefully rise away from the object before we plan another path pose = incrementPoseMsgZ(pose, 2 * object_height) # test this ik_command.service_request_pose(iksvc, pose, limb, blocking=True) else: print "Unable to plan path" continue # what to do? # Move to the stacking position group.clear_pose_targets() group.set_start_state_to_current_state() group.set_pose_target(stack_pose) plan = group.plan() plan_found = len(plan.joint_trajectory.points) > 0 if plan_found: #print "============ Waiting while RVIZ displays plan2..." #rospy.sleep(3) group.go(wait=True) gripper_if.open(block=True) group.detach_object(obj.id) # Carefully rise away from the object before we plan another path pose = incrementPoseMsgZ(stack_pose, 2 * object_height) ik_command.service_request_pose(iksvc, pose, limb, blocking=True) obj.operation = CollisionObject.REMOVE obj_manager.publish(obj) # Get the next stack pose stack_pose = incrementPoseMsgZ(stack_pose, object_height * 3 / 4) """if obj.id in obj_manager.id_operations: