p1.pose.orientation.z = b1[2] p1.pose.orientation.w = b1[3] #glovebox pose p2.pose.position.x = 0 p2.pose.position.y = 0 p2.pose.position.z = 0.56 b2 = tf.transformations.quaternion_from_euler(1.57,0,0) p2.pose.orientation.x = b2[0] p2.pose.orientation.y = b2[1] p2.pose.orientation.z = b2[2] p2.pose.orientation.w = b2[3] #detach/remove current scene objects print 'removing objects...' robot.detach_object("bowl") rospy.sleep(1) scene.remove_world_object("bowl") scene.remove_world_object("punch") scene.remove_world_object("glovebox") rospy.sleep(2) #reset the gripper and arm position to home robot.set_start_state_to_current_state() robot.set_named_target("start_glove") robot.go() gripper.set_start_state_to_current_state() gripper.go() #add scene objects
print state if (state): place_pose = PoseStamped() place_pose.header.frame_id = robot.get_planning_frame() place_pose.pose.position.x = -0.5 place_pose.pose.position.y = -0.6 place_pose.pose.position.z = 0.35 place_pose.pose.orientation.y = 1 arm.set_pose_target(place_pose) pl = arm.plan() state = arm.execute(pl) print state if (state): group_variable_values = eef.get_current_joint_values() group_variable_values[0] = 0.0 eef.detach_object("part") place_pose = PoseStamped() place_pose.header.frame_id = robot.get_planning_frame() place_pose.pose.position.x = -0.495562 place_pose.pose.position.y = -0.607059 place_pose.pose.position.z = 0.417763 place_pose.pose.orientation.y = 1 arm.set_pose_target(place_pose) pl = arm.plan() state = arm.execute(pl) print state # if(state): # place_pose = PoseStamped() # place_pose.header.frame_id = robot.get_planning_frame() # place_pose.pose.position.x = -0.5 # place_pose.pose.position.y = -0.6
class Ur10eRobot(): def __init__(self): self.GRIPPER_GROUP = "ur10e_electric_gripper" self.CAMERA_GROUP = "ur10e_camera" self.CHANGER_GROUP = "ur10e_tool_changer" self.GRIPPER_EF_GROUP = "ur10e_gripper_ef" self.GRIPPER_EF_LINK = "ur10e_gripper_ef" self.CAMERA_EF_LINK = "ur10e_camera_ef" self.CHANGER_EF_LINK = "ur10e_changer_ef" self.___PLANNING_JOINT_NAMES = [ "ur10e_shoulder_pan_joint", "ur10e_shoulder_lift_joint", "ur10e_elbow_joint", "ur10e_wrist_1_joint", "ur10e_wrist_2_joint", "ur10e_wrist_3_joint" ] self.__GRIPPER_EF_JOINT_NAMES = [ "ur10e_upper_finger_base", "ur10e_lower_finger_base" ] self.__FORCE_SENSOR_TOPIC = "/ur10e_force_topic" # create move group commander self.__gripper_group_commander = MoveGroupCommander(self.GRIPPER_GROUP) self.__camera_group_commander = MoveGroupCommander(self.CAMERA_GROUP) self.__changer_group_commander = MoveGroupCommander(self.CHANGER_GROUP) self.__gripper_ef_commander = MoveGroupCommander(self.GRIPPER_EF_GROUP) """ Update the current value from force sensor """ def update_force_sensor_value(self, force_sensor_value): self.__force_sensor_value = force_sensor_value """ Get the current value from force sensor """ def get_force_sensor_value(self): return self.__force_sensor_value """ Open electric gripper """ def open_electric_gripper(self): self.__gripper_ef_commander.set_joint_value_target([0, 0]) self.__gripper_ef_commander.go() def close_electric_gripper(self, object_width): max_width = 0.02 if (object_width > max_width): print("cannot close gripper, object width exceeds the max width") else: gripper_dist = (max_width - object_width) / 2 self.__gripper_ef_commander.set_joint_value_target( [-gripper_dist, -gripper_dist]) self.__gripper_ef_commander.go() def move_tool_in_straight_line(self, pose_goal, avoid_collisions=True, group_name=None, n_way_points=2): if (group_name == self.CAMERA_GROUP): group = self.__camera_group_commander elif (group_name == self.CHANGER_GROUP): group = self.__changer_group_commander else: group = self.__gripper_group_commander way_points_list = [] way_point = Pose() x_way_points = np.linspace(0, pose_goal[0], n_way_points) y_way_points = np.linspace(0, pose_goal[1], n_way_points) z_way_points = np.linspace(0, pose_goal[2], n_way_points) qx_way_points = np.linspace(0, 0, n_way_points) qy_way_points = np.linspace(0, 0, n_way_points) qz_way_points = np.linspace(0, 0, n_way_points) qw_way_points = np.linspace(0, 0, n_way_points) for i in range(n_way_points): way_point.position.x = x_way_points[i] way_point.position.y = y_way_points[i] way_point.position.z = z_way_points[i] way_point.orientation.x = qx_way_points[i] way_point.orientation.y = qy_way_points[i] way_point.orientation.z = qz_way_points[i] way_point.orientation.w = qw_way_points[i] way_points_list.append(way_point) group.set_pose_reference_frame(group.get_end_effector_link()) # group.set_goal_tolerance(0.001) plan, fraction = group.compute_cartesian_path(way_points_list, 0.005, 0.0) print(fraction) # print plan # post processing for the trajectory to ensure its validity last_time_step = plan.joint_trajectory.points[0].time_from_start.to_sec new_plan = RobotTrajectory() new_plan.joint_trajectory.header = plan.joint_trajectory.header new_plan.joint_trajectory.joint_names = plan.joint_trajectory.joint_names new_plan.joint_trajectory.points.append( plan.joint_trajectory.points[0]) for i in range(1, len(plan.joint_trajectory.points)): point = plan.joint_trajectory.points[i] if (point.time_from_start.to_sec > last_time_step): new_plan.joint_trajectory.points.append(point) last_time_step = point.time_from_start.to_sec # execute the trajectory group.execute(new_plan) """ Go to pose goal. Plan and execute and wait for the controller respons :param pose_goal list in this form [x, y, z, r, p, y] or [x, y, z, qx, qy, qz, qw] or [x, y, z] :type list of int :param ef_link :type: string """ def go_to_pose_goal(self, pose_goal, group_name=None): if (group_name == self.CAMERA_GROUP): group = self.__camera_group_commander elif (group_name == self.CHANGER_GROUP): group = self.__changer_group_commander else: group = self.__gripper_group_commander if (len(pose_goal) == 6 or len(pose_goal) == 7): group.set_pose_target(pose_goal) group.go() elif (len(pose_goal) == 3): group.set_position_target(pose_goal) group.go() else: print("Invalid inputs") """ Go to joint goal """ def go_to_joint_goal(self, joint_goal): if (len(joint_goal) == 6): joint_goal_msg = JointState() joint_goal_msg.name = self.___PLANNING_JOINT_NAMES joint_goal_msg.position = joint_goal self.__camera_group_commander.set_joint_value_target( joint_goal_msg) self.__camera_group_commander.go() else: print("Invalid inputs") """ pick an object using electric gripper This function assumes that the electric gripper is already in a place near to the object. With an appropriate orientation. So what is left in order to pick. is to approach the object. turn electric gripper on and then retreat. """ def pick_object(self, object_name, table_name, approach_dist, retreat_dist, object_width): # disable collision between object and table self.__gripper_group_commander.set_support_surface_name(table_name) # approach the object self.move_tool_in_straight_line(approach_dist, avoid_collisions=False) # attach object and open gripper self.__gripper_group_commander.attach_object(object_name) # close electric gripper self.close_electric_gripper(object_width) rospy.sleep(2) # retreat self.move_tool_in_straight_line(retreat_dist, avoid_collisions=True) """ Place an object using electric gripper """ def place_object(self, object_name, table_name, approach_dist, retreat_dist): # disable collision between object and table self.__gripper_group_commander.set_support_surface_name(table_name) # approach the table self.move_tool_in_straight_line(approach_dist, avoid_collisions=False) # detach object and turn off gripper self.__gripper_group_commander.detach_object(object_name) # open electric gripper self.open_electric_gripper() rospy.sleep(2) # retreat self.move_tool_in_straight_line(retreat_dist, avoid_collisions=True)
class KukaRobot: def __init__(self): self.VACUUM_GROUP = "kuka_vacuum_gripper" self.CAMERA_GROUP = "kuka_camera" self.IMPACT_GROUP = "kuka_impact_wrench" self.VACUUM_EF_LINK = "kuka_vacuum_ef" self.CAMERA_EF_LINK = "kuka_camera_ef" self.IMPACT_EF_LINK = "kuka_impact_ef" self.___PLANNING_JOINT_NAMES = [ "kuka_joint_a1", "kuka_joint_a2", "kuka_joint_a3", "kuka_joint_a4", "kuka_joint_a5", "kuka_joint_a6", "kuka_joint_a7" ] self.__FORCE_SENSOR_TOPIC = "/kuka_force_topic" self.__VACUUM_STATUS_TOPIC = "/kuka_vacuum_topic" # create move group commander self.__vacuum_group_commander = MoveGroupCommander(self.VACUUM_GROUP) self.__camera_group_commander = MoveGroupCommander(self.CAMERA_GROUP) self.__impact_group_commander = MoveGroupCommander(self.IMPACT_GROUP) # initialize vacuum services self.__vacuum_on_service = rospy.ServiceProxy("/on", Empty) self.__vacuum_off_service = rospy.ServiceProxy("/off", Empty) # initialize feedback variables self.__vacuum_status = Bool() self.__force_sensor_value = WrenchStamped() """ Update the current value from force sensor """ def update_force_sensor_value(self, force_sensor_value): self.__force_sensor_value = force_sensor_value """ Get the current value from force sensor """ def get_force_sensor_value(self): return self.__force_sensor_value """ Update Vacuum status """ def update_vacuum_status(self, vacuum_status): self.__vacuum_status = vacuum_status """ Get Vacuum status """ def get_vacuum_status(self): return self.__vacuum_status """ Turn on Vacuum gripper """ def vacuum_gripper_on(self): self.__vacuum_on_service.wait_for_service(0.2) request = EmptyRequest() self.__vacuum_on_service(request) """ Turn off Vacuum gripper """ def vacuum_gripper_off(self): self.__vacuum_off_service.wait_for_service(0.2) request = EmptyRequest() self.__vacuum_off_service(request) """ move in a straight line with respect to the tool reference frame """ def move_tool_in_straight_line(self, pose_goal, avoid_collisions=True, group_name=None, n_way_points=2): if group_name == self.CAMERA_GROUP: group = self.__camera_group_commander elif group_name == self.IMPACT_GROUP: group = self.__impact_group_commander else: group = self.__vacuum_group_commander way_points_list = [] way_point = Pose() x_way_points = np.linspace(0, pose_goal[0], n_way_points) y_way_points = np.linspace(0, pose_goal[1], n_way_points) z_way_points = np.linspace(0, pose_goal[2], n_way_points) qx_way_points = np.linspace(0, 0, n_way_points) qy_way_points = np.linspace(0, 0, n_way_points) qz_way_points = np.linspace(0, 0, n_way_points) qw_way_points = np.linspace(0, 0, n_way_points) for i in range(n_way_points): way_point.position.x = x_way_points[i] way_point.position.y = y_way_points[i] way_point.position.z = z_way_points[i] way_point.orientation.x = qx_way_points[i] way_point.orientation.y = qy_way_points[i] way_point.orientation.z = qz_way_points[i] way_point.orientation.w = qw_way_points[i] way_points_list.append(way_point) group.set_pose_reference_frame(group.get_end_effector_link()) # group.set_goal_tolerance(0.001) plan, fraction = group.compute_cartesian_path(way_points_list, 0.005, 0.0, avoid_collisions) print(fraction) # print plan # post processing for the trajectory to ensure its validity last_time_step = plan.joint_trajectory.points[0].time_from_start.to_sec new_plan = RobotTrajectory() new_plan.joint_trajectory.header = plan.joint_trajectory.header new_plan.joint_trajectory.joint_names = plan.joint_trajectory.joint_names new_plan.joint_trajectory.points.append( plan.joint_trajectory.points[0]) for i in range(1, len(plan.joint_trajectory.points)): point = plan.joint_trajectory.points[i] if point.time_from_start.to_sec > last_time_step: new_plan.joint_trajectory.points.append(point) last_time_step = point.time_from_start.to_sec # execute the trajectory group.execute(new_plan) """ Go to pose goal. Plan and execute and wait for the controller response :param pose_goal list in this form [x, y, z, r, p, y] or [x, y, z, qx, qy, qz, qw] or [x, y, z] :type list of int :param ef_link :type: string """ def go_to_pose_goal(self, pose_goal, group_name=None): if group_name == self.CAMERA_GROUP: group = self.__camera_group_commander elif group_name == self.IMPACT_GROUP: group = self.__impact_group_commander else: group = self.__vacuum_group_commander if len(pose_goal) == 6 or len(pose_goal) == 7: group.set_pose_target(pose_goal) group.go() elif len(pose_goal) == 3: group.set_position_target(pose_goal) group.go() else: print("Invalid inputs") """ Go to joint goal """ def go_to_joint_goal(self, joint_goal): if len(joint_goal) == 7: joint_goal_msg = JointState() joint_goal_msg.name = self.___PLANNING_JOINT_NAMES joint_goal_msg.position = joint_goal self.__camera_group_commander.set_joint_value_target( joint_goal_msg) self.__camera_group_commander.go() else: print("Invalid inputs") """ pick an object using vacuum gripper This function assumes that the vacuum gripper is already in a place near to the object. With an appropriate orientation. So what is left in order to pick. is to approach the object. turn vacuum gripper on and then retreat. """ def pick_object(self, object_name, table_name, approach_dist, retreat_dist): # disable collision between object and table self.__vacuum_group_commander.set_support_surface_name(table_name) # approach the object self.move_tool_in_straight_line(approach_dist, avoid_collisions=True) # attach object and open gripper self.__vacuum_group_commander.attach_object(object_name) # turn on vacuum gripper self.vacuum_gripper_on() rospy.sleep(2) # retreat self.move_tool_in_straight_line(retreat_dist, avoid_collisions=True) """ Place an object using vacuum gripper """ def place_object(self, object_name, table_name, approach_dist, retreat_dist): # disable collision between object and table self.__vacuum_group_commander.set_support_surface_name(table_name) # approach the table self.move_tool_in_straight_line(approach_dist, avoid_collisions=True) # detach object and turn off gripper self.__vacuum_group_commander.detach_object(object_name) # turn off vacuum gripper self.vacuum_gripper_off() rospy.sleep(2) # retreat self.move_tool_in_straight_line(retreat_dist, avoid_collisions=True)
p.pose.position.x = 0.72 p.pose.position.z = 0.05 # add the pen scene.add_mesh("pen",p,resourcepath+'objects/pen.dae') rospy.sleep(1) # print the existing groups robot = RobotCommander() print "Available groups: ",robot.get_group_names() # setup the arm group and its planner arm = MoveGroupCommander("arm") arm.set_start_state_to_current_state() arm.set_planner_id("RRTstarkConfigDefault") arm.set_planning_time(5.0) arm.detach_object("pen") # set the arm to a safe target arm.set_named_target("gamma") # plan and execute the motion arm.go() # setup the hand group and its planner hand = MoveGroupCommander("hand") hand.set_start_state_to_current_state() hand.set_planner_id("LBKPIECEkConfigDefault") hand.set_planning_time(10.0) # set the hand to a safe target hand.set_named_target("open") # plan and execute the motion
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') gripper = MoveGroupCommander('gripper') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' # 移除场景中之前运行残留的物体 scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_attached_object(box2_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 '''arm.set_named_target('home') arm.go() rospy.sleep(2)''' # 设置桌面的高度 table_ground = 0.6 # 设置table、box1和box2的三维尺寸 table_size = [1.5, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.05] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.0 table_pose.pose.position.y = 0.5 table_pose.pose.position.z = 0.825 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.2 box1_pose.pose.position.y = 0.3 box1_pose.pose.position.z = table_ground + table_size[ 2] + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 #scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = -0.1 box2_pose.pose.position.y = 0.5 box2_pose.pose.position.z = 0.85 box2_pose.pose.orientation.w = 1.0 #scene.add_box(box2_id, box2_pose, box2_size) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 0, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.3, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置机械臂的运动目标位置,进行避障规划 orient = transformations.quaternion_from_euler(0, 1.57075, 0) target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = -0.1 target_pose.pose.position.y = 0.5 target_pose.pose.position.z = 1.05 target_pose.pose.orientation.x = orient[0] target_pose.pose.orientation.y = orient[1] target_pose.pose.orientation.z = orient[2] target_pose.pose.orientation.w = orient[3] # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(0.5) target_pose.pose.position.z -= 0.05 arm.set_pose_target(target_pose, end_effector_link) arm.go() self.gripper_control(0.355) '''arm.attach_object( box2_id) arm.set_named_target('home') arm.go()''' #self.gripper_control(0) arm.detach_object(box2_id) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
p.pose.position.x = 0.72 p.pose.position.z = 0.05 # add the pen scene.add_mesh("pen", p, resourcepath + 'objects/pen.dae') rospy.sleep(1) # print the existing groups robot = RobotCommander() print "Available groups: ", robot.get_group_names() # setup the arm group and its planner arm = MoveGroupCommander("arm") arm.set_start_state_to_current_state() arm.set_planner_id("RRTstarkConfigDefault") arm.set_planning_time(5.0) arm.detach_object("pen") # set the arm to a safe target arm.set_named_target("gamma") # plan and execute the motion arm.go() # setup the hand group and its planner hand = MoveGroupCommander("hand") hand.set_start_state_to_current_state() hand.set_planner_id("LBKPIECEkConfigDefault") hand.set_planning_time(10.0) # set the hand to a safe target hand.set_named_target("open") # plan and execute the motion
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) #self.robot_arm.set_num_planning_attempts(5) self.robot_arm.remember_joint_values("zeros", None) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) #add Tabl p = PoseStamped() p.header.frame_id = FIXED_FRAME p.pose.position.x = 0.3 p.pose.position.y = 0.2 p.pose.position.z = 0.1 self.scene.add_box("table", p, (0.02, 0.02, 0.02)) pose_goal.position.x = 0.3 pose_goal.position.y = 0.2 pose_goal.position.z = 0.15 #Orientation = [1.57,0,0] Orientation[0] = 0 Orientation[1] = 0 Orientation[2] = 1.57 def WASD(self): quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) rospy.sleep(2) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] rospy.sleep(2) self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) print("trying done") print("\n") print("WASD keyboard control") while (True): print("Available commands : ") print(" W - move X forward") print(" S - move X backward") print(" A - move Y left") print(" D - move Y right") print(" R - move Z left") print(" F - move Z right") print("*****************") print(" X - pick object") print(" Y - pick object") print("current orient", Orientation) command = raw_input("Control : ") if command == 'w': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x + 0.05 pose_goal.position.y = pose_goal.position.y pose_goal.position.z = pose_goal.position.z pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 's': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x - 0.05 pose_goal.position.y = pose_goal.position.y pose_goal.position.z = pose_goal.position.z pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'a': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x pose_goal.position.y = pose_goal.position.y - 0.05 pose_goal.position.z = pose_goal.position.z pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'd': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x pose_goal.position.y = pose_goal.position.y + 0.05 pose_goal.position.z = pose_goal.position.z pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'r': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x pose_goal.position.y = pose_goal.position.y pose_goal.position.z = pose_goal.position.z + 0.05 pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'f': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x pose_goal.position.y = pose_goal.position.y pose_goal.position.z = pose_goal.position.z - 0.05 pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'x': self.robot_arm.attach_object("table") elif command == 'y': self.robot_arm.detach_object("table") elif command == 'v': self.set_pose_target(pose_goal) elif command == 'i': Orientation[0] = Orientation[0] + 0.2 Orientation[1] = Orientation[1] Orientation[2] = Orientation[2] quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'k': Orientation[0] = Orientation[0] - 0.2 Orientation[1] = Orientation[1] Orientation[2] = Orientation[2] quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) # yaw elif command == 'u': Orientation[0] = Orientation[0] Orientation[1] = Orientation[1] + 0.2 Orientation[2] = Orientation[2] quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'o': Orientation[0] = Orientation[0] Orientation[1] = Orientation[1] - 0.2 Orientation[2] = Orientation[2] quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) # roll elif command == 'j': Orientation[0] = Orientation[0] Orientation[1] = Orientation[1] Orientation[2] = Orientation[2] + 0.2 quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'l': Orientation[0] = Orientation[0] Orientation[1] = Orientation[1] Orientation[2] = Orientation[2] - 0.2 quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.robot_arm.remember_joint_values("zeros", None) rospy.sleep(2) # Allow replanning to increase the odds of a solution #self.robot_arm.allow_replanning(True) #scene = moveit_commander.PlanningSceneInterface() #robot = moveit_commander.RobotCommander() #rospy.sleep(2) #add Table p = PoseStamped() p.header.frame_id = FIXED_FRAME p.pose.position.x = 0.0 p.pose.position.y = 0.2 p.pose.position.z = 0.1 self.scene.add_box("table", p, (0.3, 0.3, 0.3)) def move_code(self): self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home 1 ======") rospy.sleep(1) self.robot_arm.attach_object("table") pose_goal.orientation.w = 0.0 pose_goal.position.x = 0.4 # red line 0.2 0.2 pose_goal.position.y = 0.15 # green line 0.15 0.15 pose_goal.position.z = 0.2 # blue line # 0.35 0.6 self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) #if (input == 'w') #pose_goal.position.x += 0.1 # red line 0.2 0.2 print("====== move plan go to up ======") self.robot_arm.set_named_target("zeros") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to zeros ======") # rospy.sleep(1) #self.robot_arm.place("table", pose_goal) # robot_arm.set_named_target("up") # robot_arm.go(wait=True) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() self.robot_arm.detach_object("table") pose_goal.orientation.w = 0.0 pose_goal.position.x = 0.2 # red line 0.2 0.2 pose_goal.position.y = 0.3 # green line 0.15 0.15 pose_goal.position.z = 0.1 # blue line # 0.35 0.6 self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) print(robot_state)