def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('start_pos') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 10 seconds per planning attempt right_arm.set_planning_time(10) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def talker_by13(): #init moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_fk_demo') #cartesian = rospy.get_param('~cartesian', True) arm = MoveGroupCommander('manipulator') arm.set_pose_reference_frame('base_link') arm.allow_replanning(True) arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # arm.set_max_acceleration_scaling_factor(0.5) #arm.set_max_velocity_scaling_factor(0.5) end_effector_link = arm.get_end_effector_link() #arm.set_named_target('home') arm.set_named_target('up') arm.go() rospy.sleep(2) target_pose = PoseStamped() target_pose.header.frame_id = 'base_link' target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.86 target_pose.pose.position.y = 0.25 target_pose.pose.position.z = 0.02832 target_pose.pose.orientation.x = 0 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = 0 target_pose.pose.orientation.w = 1 #next: find workspace arm.set_start_state_to_current_state() arm.set_pose_target(target_pose, end_effector_link) traj = arm.plan() arm.execute(traj) #arm.shift_pose_target(2,-0.05,end_effector_link) #arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.0001) arm.set_goal_orientation_tolerance(0.0001) arm.set_max_velocity_scaling_factor(0.5) # 获取终端link的名称 eef_link = arm.get_end_effector_link() scene = moveit_commander.PlanningSceneInterface() print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose # 控制机械臂运动到之前设置的姿态 # arm.set_named_target('pick_6') arm.set_named_target('home') arm.go() self.box_name = '' self.scene = scene self.group = arm self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander self.move_distance = 0.1 self.back_distance = 0.15
class Robot: def __init__(self, config, debug=0): print "[INFO] Initialise Robot" self.DEBUG = debug # configuration self.config = config # initialise move groups self.arm = MoveGroupCommander(ARM_GROUP) self.gripper = MoveGroupCommander(GRIPPER_GROUP) # initialise pick and place manager if self.DEBUG is 1: # verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True) else: # non verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False) # tolerance: position (in m), orientation (in rad) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) self.place_manager = None self.pick_manager = None self.initialise_move_actions() self.start_position() """Initialise the place and pick manager. """ def initialise_move_actions(self): self.place_manager = PlaceManager(self.arm, self.pick_and_place, self.config, self.DEBUG) self.pick_manager = PickManager(self.arm, self.pick_and_place, self.DEBUG) """Move robot arm to "start position". """ def start_position(self): self.arm.set_named_target(START_POSITION) self.arm.go() rospy.sleep(2)
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) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) 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) # print("====== move plan go to up ======") self.robot_arm.set_named_target("table") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to table 1 ======") rospy.sleep(1) range(0, 5) rospy.spin() roscpp_shutdown() # robot_arm.set_named_target("up") # robot_arm.go(wait=True) robot_state = robot_arm.get_current_pose() robot_angle = robot_arm.get_current_joint_values() print(robot_state)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_move_position', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) arm.set_planning_time(10) arm.set_named_target(START_POSITION) arm.go() rospy.sleep(2) print "======== create new goal position ========" start_pose = PoseStamped() start_pose.header.frame_id = arm.get_planning_frame() # Test Position start_pose.pose.position.x = 0.2 # 20 cm start_pose.pose.position.y = -0.11 # -11 cm start_pose.pose.position.z = 0.6 # 60 cm q = quaternion_from_euler(0.0, 0.0, 0.0) start_pose.pose.orientation = Quaternion(*q) print start_pose print "====== move to position =======" # KDL # arm.set_joint_value_target(start_pose, True) # IK Fast arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z], arm.get_end_effector_link()) arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def talker_by13(): #init moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_fk_demo') cartesian = rospy.get_param('~cartesian', True) arm = MoveGroupCommander('manipulator') #arm.set_pose_reference_frame('base_link') #arm.set_goal_position_tolerance(0.001) # arm.set_goal_orientation_tolerance(0.001) # arm.set_max_acceleration_scaling_factor(0.5) #arm.set_max_velocity_scaling_factor(0.5) end_effector_link = arm.get_end_effector_link() arm.set_named_target('home') arm.go() rospy.sleep(1) start_pose = arm.get_current_pose(end_effector_link).pose waypoints = [] wpose = deepcopy(start_pose) wpose.position.x -= 0.2 wpose.position.y -= 0.2 #wpose.position.z += 0.6 #wpose.position.x = 0.8 #wpose.position.y = 0.9 #wpose.position.z = 0.9 arm.set_pose_target(wpose) arm.go() rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def main(): rospy.init_node('arm_start_up', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") left_arm = MoveGroupCommander("left_arm") right_arm.set_planner_id("RRTConnect:"); left_arm.set_planner_id("RRTConnect:") # move to a random target #group.set_named_target("ahead") #group.go() #rospy.sleep(1) right_arm.set_named_target("right_start") right_arm.go() rospy.sleep(1) left_arm.set_named_target("left_start") left_arm.go() rospy.sleep(1)
class Robot: def __init__(self, config, debug=0): print "[INFO] Initialise Robot" self.DEBUG = debug # configuration self.config = config # initialise move groups self.arm = MoveGroupCommander(ARM_GROUP) self.gripper = MoveGroupCommander(GRIPPER_GROUP) # initialise pick and place manager if self.DEBUG is 1: # verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True) else: # non verbose mode self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False) # tolerance: position (in m), orientation (in rad) self.arm.set_goal_position_tolerance(0.01) self.arm.set_goal_orientation_tolerance(0.1) self.place_manager = None self.pick_manager = None self.initialise_move_actions() self.start_position() """Initialise the place and pick manager. """ def initialise_move_actions(self): self.place_manager = PlaceManager(self.arm, self.pick_and_place, self.config, self.DEBUG) self.pick_manager = PickManager(self.arm, self.pick_and_place, self.DEBUG) """Move robot arm to "start position". """ def start_position(self): self.arm.set_named_target(START_POSITION) self.arm.go() rospy.sleep(2)
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.pose_goal = Pose() rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) 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(0.5) # print("====== move plan go to up ======") self.robot_arm.set_named_target("up") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to up ======") rospy.sleep(0.5) 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(0.5) # 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() print(robot_state) def move_TF(self): self.ee_TF_list = [-2.046, 1.121, 0.575, 0.453, 0.561, -0.435, 0.538] self.pose_goal.position.x = self.ee_TF_list[0] self.pose_goal.position.y = self.ee_TF_list[1] self.pose_goal.position.z = self.ee_TF_list[2] self.pose_goal.orientation.x = self.ee_TF_list[3] self.pose_goal.orientation.y = self.ee_TF_list[4] self.pose_goal.orientation.z = self.ee_TF_list[5] self.pose_goal.orientation.w = self.ee_TF_list[6] self.robot_arm.set_pose_target(self.pose_goal) self.robot_arm.go(True)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') robot = moveit_commander.RobotCommander() # 当运动规划失败后,允许重新规划 arm.allow_replanning(False) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.0001) arm.set_goal_orientation_tolerance(0.0001) arm.set_max_velocity_scaling_factor(1.0) arm.set_planning_time(0.05) # 规划时间限制 # 获取终端link的名称 eef_link = arm.get_end_effector_link() print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose # 控制机械臂运动到之前设置的姿态 arm.set_named_target('work') arm.go() self.group = arm self.robot = robot self.eef_link = eef_link self.reference_frame = reference_frame self.moveit_commander = moveit_commander
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_speed_demo') # Initialize the move group for the right arm arm = MoveGroupCommander('manipulator') # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Allow some leeway in position(meters) and orientation (radians) arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.01) # Start the arm in the "home" pose stored in the SRDF file arm.set_named_target('home') arm.go() # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() # Start the arm in the "home" pose stored in the SRDF file arm.set_named_target('home') arm.go() # Get back the planned trajectory arm.set_joint_value_target(joint_positions) traj = arm.plan() # Scale the trajectory speed by a factor of 0.25 new_traj = scale_trajectory_speed(traj, 0.25) # Execute the new trajectory arm.execute(new_traj) rospy.sleep(1) arm.set_named_target('home') arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.02) right_arm.set_goal_orientation_tolerance(0.1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('rest') right_arm.go() # Move to the named "straight_forward" pose stored in the SRDF file right_arm.set_named_target('wave') right_arm.go() rospy.sleep(2) # Move back to the resting position at roughly 1/4 speed right_arm.set_named_target('rest') # Get back the planned trajectory traj = right_arm.plan() # Set the trajectory speed new_traj = create_tracking_trajectory(traj, 1.0) rospy.loginfo(new_traj) # Execute the new trajectory right_arm.execute(new_traj) rospy.sleep(1) # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.02) right_arm.set_goal_orientation_tolerance(0.1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() # Move to the named "straight_forward" pose stored in the SRDF file right_arm.set_named_target('straight_forward') right_arm.go() rospy.sleep(2) # Move back to the resting position at roughly 1/4 speed right_arm.set_named_target('resting') # Get back the planned trajectory traj = right_arm.plan() # Scale the trajectory speed by a factor of 0.25 new_traj = scale_trajectory_speed(traj, 0.25) # Execute the new trajectory right_arm.execute(new_traj) rospy.sleep(1) # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('gripper_test') # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') arm.set_max_velocity_scaling_factor(0.3) arm.set_max_acceleration_scaling_factor(0.5) # 初始化需要使用move group控制的机械臂中的gripper group gripper = MoveGroupCommander('gripper') self.gripper = gripper # 控制机械臂先回到初始位置 arm.set_named_target('home') arm.go() # 控制夹爪张开 gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(0.5) # 控制机械臂到目标位置 arm.set_named_target('test_2') arm.go() # 控制夹爪闭合 gripper.set_joint_value_target(GRIPPER_CLOSED) gripper.go() # 控制机械臂先回到初始位置 arm.set_named_target('home') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(0.5) arm.set_max_velocity_scaling_factor(0.5) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 获取当前位姿数据最为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose print start_pose # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 waypoints.append(start_pose) # 设置路点数据,并加入路点列表 wpose = deepcopy(start_pose) wpose.position.z -= 0.2 waypoints.append(deepcopy(wpose)) wpose.position.x += 0.1 waypoints.append(deepcopy(wpose)) wpose.position.y += 0.1 waypoints.append(deepcopy(wpose)) fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
if __name__ == '__main__': # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node("cobra_move_pick") arm = MoveGroupCommander(PLANNING_GROUP) # # tolerance # arm.set_goal_position_tolerance(0.01) # arm.set_goal_orientation_tolerance(0.01) # arm.set_goal_joint_tolerance(0.01) # arm.set_goal_tolerance(0.01) # move arm to start position arm.set_named_target(START_POSITION) arm.go() rospy.sleep(2) # Action Clients for Pick rospy.loginfo("Connecting to pickup AS") pickup_ac = actionlib.SimpleActionClient('/pickup', PickupAction) pickup_ac.wait_for_server() rospy.loginfo("Successfully connected") # create Planning Scene scene = PlanningSceneInterface() rospy.sleep(1) # clean world rospy.loginfo("Cleaning world objects")
punch_pose.pose.position.z = 0 temp_eul = None #detach/remove current scene objects print 'removing objects...' robot.detach_object("bowl") rospy.sleep(0.5) scene.remove_world_object("bowl") scene.remove_world_object("punch") scene.remove_world_object("flat") rospy.sleep(0.5) #reset the gripper and arm position to home robot.set_start_state_to_current_state() robot.set_named_target("home") robot.go() startpose = PoseStamped() startpose.header.frame_id = 'world' startpose.header.stamp = rospy.Time.now() startpose = robot.get_current_pose() startpose.pose.position.x -= 0.3 robot.set_pose_target(startpose) robot.go() # gripper.set_start_state_to_current_state() # gripper.set_named_target("default") # gripper.go() #add scene objects print 'adding scene objects' scene.add_mesh("bowl", bowl_pose, "8inhemi.STL")
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_constraints_demo', anonymous=True) robot = RobotCommander() # Connect to the arm move group arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since constraint planning can take a while arm.set_planning_time(5) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file arm.set_named_target('l_arm_init') # Plan and execute a trajectory to the goal configuration arm.go() rospy.sleep(1) # Open the gripper gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.263803774718 target_pose.pose.position.y = 0.295405791959 target_pose.pose.position.z = 0.690438884208 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the start state and target pose, then plan and execute arm.set_start_state(robot.get_current_state()) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) # Close the gripper gripper.set_joint_value_target(GRIPPER_CLOSED) gripper.go() rospy.sleep(1) # Store the current pose start_pose = arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 1.0 # q = quaternion_from_euler(0, 0, -1.57079633) # orientation_constraint.orientation.x = q[0] # orientation_constraint.orientation.y = q[1] # orientation_constraint.orientation.z = q[2] # orientation_constraint.orientation.w = q[3] # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the arm arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.39000848183 target_pose.pose.position.y = 0.185900663329 target_pose.pose.position.z = 0.732752341378 target_pose.pose.orientation.w = 1 # Set the start state and target pose, then plan and execute arm.set_start_state_to_current_state() arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) # Clear all path constraints arm.clear_path_constraints() # Open the gripper gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file arm.set_named_target('l_arm_init') # Plan and execute a trajectory to the goal configuration arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def callback(self, data): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 scene = PlanningSceneInterface() 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' # 控制机械臂先回到初始化位置 '''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] # 将三个物体加入场景当中 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) rospy.sleep(1) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0.8, 0, 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]''' orient = transformations.quaternion_from_euler(0, 1.57075, 0) target_pose = data 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] # 控制机械臂运动到目标位置 self.gripper_control(0) result = None arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.loginfo('Start grasping') target_pose.pose.position.z -= 0.05 arm.set_pose_target(target_pose, end_effector_link) arm.go() self.gripper_control(0.36) '''arm.attach_object( box2_id) arm.set_named_target('home') arm.go()''' arm.set_named_target('home') arm.go() self.gripper_control(0) rospy.loginfo('Grasping done') rospy.sleep(2) judge_client(True) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
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=5) rospy.Subscriber("chatter", Float64MultiArray, callback) # 创建一个存储物体颜色的字典对象 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) gripper.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.2) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' # cy_id = 'cy' box1_id = 'box1' box2_id = 'box2' box3_id = 'box3' sphere_id = 'sphere' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(box3_id) scene.remove_world_object(sphere_id) rospy.sleep(1) #控制机械臂先回到初始化位置 arm.set_named_target('init') arm.go() rospy.sleep(1) # arm.set_named_target('start') # arm.go() # rospy.sleep(1) gripper.set_joint_value_target([0.05]) gripper.go() rospy.sleep(0) # 设置桌面的高度 table_ground = 0.37 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.3, 0.01] box1_size = [0.01, 0.01, 0.19] box2_size = [0.01, 0.01, 0.19] box3_size = [0.005, 0.01, 0.3] sphere_R = 0.01 error = 0.03 # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = -table_size[0] / 2.0 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) #scene.add_cylinder box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = -0.09 box1_pose.pose.position.y = table_size[0] / 2.0 box1_pose.pose.position.z = 0.18 + 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.09 box2_pose.pose.position.y = -table_size[0] / 2.0 box2_pose.pose.position.z = 0.18 + box1_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # box3_pose = PoseStamped() # box3_pose.header.frame_id = reference_frame # box3_pose.pose.position.x = pos_aim[0] # box3_pose.pose.position.y = pos_aim[1] # box3_pose.pose.position.z = box3_size[2]/2.0+0.1 # box3_pose.pose.orientation.w = 1.0 # scene.add_box(box3_id, box3_pose, box3_size) sphere_pose = PoseStamped() sphere_pose.header.frame_id = reference_frame sphere_pose.pose.position.x = pos_aim[0] + 0 sphere_pose.pose.position.y = pos_aim[1] sphere_pose.pose.position.z = pos_aim[2] sphere_pose.pose.orientation.w = 1.0 scene.add_sphere(sphere_id, sphere_pose, sphere_R) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0, 0, 0, 1) # self.setColor(table_id, 0.8, 0, 0, 1.0) # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box1_id, 1, 1, 1, 1.0) self.setColor(box2_id, 1, 1, 1, 1.0) self.setColor(box3_id, 1, 1, 1, 1.0) self.setColor(sphere_id, 0.8, 0, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # rospy.INFO("waiting...") # if(Recive_FLAG==1): # rospy.INFO("OK!") # 设置机械臂的运动目标位置 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = pos_aim[0] - error target_pose.pose.position.y = pos_aim[1] target_pose.pose.position.z = pos_aim[2] # target_pose.pose.orientation.w = 1.0 #####0.3 # 0.2 0.2 0.2 0.15 0.15 # 0.12 0.11 0.12 0.15 0.15 # 0.30 0.245 0.35 0.35 0.25 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) gripper.set_joint_value_target([0.03]) gripper.go() rospy.sleep(1) # # 控制机械臂终端向x移动5cm arm.shift_pose_target(0, 0.01, end_effector_link) arm.go() rospy.sleep(1) gripper.set_joint_value_target([0.05]) gripper.go() rospy.sleep(1) # # 设置机械臂的运动目标位置,进行避障规划 # target_pose2 = PoseStamped() # target_pose2.header.frame_id = reference_frame # target_pose2.pose.position.x = 0.15 # target_pose2.pose.position.y = 0 #-0.25 # target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 # target_pose2.pose.orientation.w = 1.0 # # 控制机械臂运动到目标位置 # arm.set_pose_target(target_pose2, end_effector_link) # arm.go() # rospy.sleep(2) #控制机械臂回到初始化位置 arm.set_named_target('init') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) rospy.spin()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening")] self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening")] self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral")] self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten") # We need a tf listener to convert poses into arm reference base self.tf_listener = tf.TransformListener() # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.04) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 3 # Set a limit on the number of place attempts max_place_attempts = 3 rospy.loginfo("Scaling for MoveIt timeout=" + str( rospy.get_param( '/move_group/trajectory_execution/allowed_execution_duration_scaling' ))) # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name #table_id = 'table' We also remove the table object in order to run a test #box1_id = 'box1' These boxes are commented as we do not need them #box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run #scene.remove_world_object(table_id) #scene.remove_world_object(box1_id) These boxes are commented as we do not need them #scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "arm_up" pose stored in the SRDF file rospy.loginfo("Set Arm: right_up") arm.set_named_target('right_up') if arm.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the closed position rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed)) gripper.set_joint_value_target(self.gripper_closed) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the neutral position rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral)) gripper.set_joint_value_target(self.gripper_neutral) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Move the gripper to the open position rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened)) gripper.set_joint_value_target(self.gripper_opened) if gripper.go() != True: rospy.logwarn(" Go failed") rospy.sleep(2) # Set the height of the table off the ground #table_ground = 0.4 # Set the dimensions of the scene objects [l, w, h] #table_size = [0.2, 0.7, 0.01] #box1_size = [0.1, 0.05, 0.05] commented for the same reasons as previously #box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [ 0.018, 0.018, 0.018 ] #[0.02, 0.005, 0.12] original object dimensions in meters # Add a table top and two boxes to the scene #table_pose = PoseStamped() #table_pose.header.frame_id = REFERENCE_FRAME #table_pose.pose.position.x = 0.36 #table_pose.pose.position.y = 0.0 #table_pose.pose.position.z = table_ground + table_size[2] -0.08 / 2.0 #0.4+0.01/2 aka table_ground + table_size[2] + target_size[2] / 2.0 #table_pose.pose.orientation.w = 1.0 #scene.add_box(table_id, table_pose, table_size) #box1_pose = PoseStamped() These two blocks of code are commented as they assign postion to unwanted boxes #box1_pose.header.frame_id = REFERENCE_FRAME #box1_pose.pose.position.x = table_pose.pose.position.x - 0.04 #box1_pose.pose.position.y = 0.0 #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 = table_pose.pose.position.x - 0.06 #box2_pose.pose.position.y = 0.2 #box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 #box2_pose.pose.orientation.w = 1.0 #scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = -0.03 #table_pose.pose.position.x - 0.03 target_pose.pose.position.y = 0.1 target_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2 #table_ground + table_size[2] + target_size[2] / 2.0 table_ground + table_size[2] + target_size[2] -0.08 / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange #self.setColor(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.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object #arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = -0.03 #table_pose.pose.position.x - 0.03 place_pose.pose.position.y = -0.15 place_pose.pose.position.z = 0.4 + 0.01 + 0.018 - 0.08 / 2 #table_ground + table_size[2] + target_size[2] -0.08 / 2.0 0.4+0.01+0.018/2 aka table_ground + table_size[2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten]) # Track success/failure and number of attempts for pick operation result = MoveItErrorCodes.FAILURE n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: rospy.loginfo("Pick attempt #" + str(n_attempts)) for grasp in grasps: # Publish the grasp poses so they can be viewed in RViz self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) result = arm.pick(target_id, grasps) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: rospy.loginfo(" Pick: Done!") # Generate valid place poses places = self.make_places(place_pose) success = False #from here we are forcing the success cases by stting and making it always true it makes an error appear and the arm stays to the middle position n_attempts = 0 # Repeat until we succeed or run out of attempts while not success and n_attempts < max_place_attempts: #while not has been removed added = after operator < rospy.loginfo("Place attempt #" + str(n_attempts)) for place in places: # Publish the place poses so they can be viewed in RViz self.gripper_pose_pub.publish(place) rospy.sleep(0.2) success = arm.place(target_id, place) break if success: break n_attempts += 1 rospy.sleep(0.2) if not success: rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.") else: #end of forcing rospy.loginfo(" Place: Done!") else: rospy.logerr("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file (passing through right_up) arm.set_named_target('right_up') arm.go() arm.set_named_target('resting') arm.go() # Open the gripper to the neutral position gripper.set_joint_value_target(self.gripper_neutral) gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) robot = RobotCommander() # Connect to the right_arm move group right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since constraint planning can take a while right_arm.set_planning_time(15) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file right_arm.set_named_target('right_arm_zero') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.237012590198 target_pose.pose.position.y = -0.0747191267505 target_pose.pose.position.z = 0.901578401949 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state(robot.get_current_state()) right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # Close the gripper right_gripper.set_joint_value_target(GRIPPER_CLOSED) right_gripper.go() rospy.sleep(1) # Store the current pose start_pose = right_arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = right_arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the right_arm right_arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.173187824708 target_pose.pose.position.y = -0.0159929871606 target_pose.pose.position.z = 0.692596608605 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state_to_current_state() right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(1) # Clear all path constraints right_arm.clear_path_constraints() # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('right_arm_zero') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) robot = RobotCommander() # Connect to the right_arm move group right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since contraint planning can take a while right_arm.set_planning_time(15) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.237012590198 target_pose.pose.position.y = -0.0747191267505 target_pose.pose.position.z = 0.901578401949 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state(robot.get_current_state()) right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # Close the gripper right_gripper.set_joint_value_target(GRIPPER_CLOSED) right_gripper.go() rospy.sleep(1) # Store the current pose start_pose = right_arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = right_arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the right_arm right_arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.173187824708 target_pose.pose.position.y = -0.0159929871606 target_pose.pose.position.z = 0.692596608605 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state_to_current_state() right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(1) # Clear all path constraints right_arm.clear_path_constraints() # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
#!/usr/bin/env python import rospy from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander if __name__ == '__main__': rospy.init_node('heh') scene = PlanningSceneInterface() robot = MoveGroupCommander("sia5d"); gripper = MoveGroupCommander("gripper"); rospy.sleep(1) while not rospy.is_shutdown(): gripper.set_named_target('pinch') gripper.go() rospy.sleep(1) gripper.set_named_target('default') gripper.go() rospy.sleep(1) rospy.spin()
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") right_gripper = MoveGroupCommander("right_gripper") eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object("right_gripper_link", "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") right_arm.set_named_target("start1") right_arm.go() right_gripper.set_named_target("open") right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table #p.pose.position.x = 0.42 #p.pose.position.y = -0.2 #p.pose.position.z = 0.3 #scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.15 p.pose.position.y = -0.12 p.pose.position.z = 0.7 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.0130178 start_pose.pose.position.y = -0.125155 start_pose.pose.position.z = 0.597653 start_pose.pose.orientation.x = 0.0 start_pose.pose.orientation.y = 0.388109 start_pose.pose.orientation.z = 0.0 start_pose.pose.orientation.w = 0.921613 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps #grasps = self.make_grasps(start_pose) #result = False #n_attempts = 0 # repeat until will succeed #while result == False: #result = robot.right_arm.pick("part", grasps) #n_attempts += 1 #print "Attempts: ", n_attempts #rospy.sleep(0.2) rospy.spin() roscpp_shutdown()
class SmartGrasper(object): """ This is the helper library to easily access the different functionalities of the simulated robot from python. """ __last_joint_state = None __current_model_name = "cricket_ball" __path_to_models = "/root/.gazebo/models/" def __init__(self): """ This constructor initialises the different necessary connections to the topics and services and resets the world to start in a good position. """ rospy.init_node("smart_grasper") self.__joint_state_sub = rospy.Subscriber("/joint_states", JointState, self.__joint_state_cb, queue_size=1) rospy.wait_for_service("/gazebo/get_model_state", 10.0) rospy.wait_for_service("/gazebo/reset_world", 10.0) self.__reset_world = rospy.ServiceProxy("/gazebo/reset_world", Empty) self.__get_pose_srv = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState) rospy.wait_for_service("/gazebo/pause_physics") self.__pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty) rospy.wait_for_service("/gazebo/unpause_physics") self.__unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty) rospy.wait_for_service("/controller_manager/switch_controller") self.__switch_ctrl = rospy.ServiceProxy("/controller_manager/switch_controller", SwitchController) rospy.wait_for_service("/gazebo/set_model_configuration") self.__set_model = rospy.ServiceProxy("/gazebo/set_model_configuration", SetModelConfiguration) rospy.wait_for_service("/gazebo/delete_model") self.__delete_model = rospy.ServiceProxy("/gazebo/delete_model", DeleteModel) rospy.wait_for_service("/gazebo/spawn_sdf_model") self.__spawn_model = rospy.ServiceProxy("/gazebo/spawn_sdf_model", SpawnModel) rospy.wait_for_service('/get_planning_scene', 10.0) self.__get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.__pub_planning_scene = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10, latch=True) self.arm_commander = MoveGroupCommander("arm") self.hand_commander = MoveGroupCommander("hand") self.__hand_traj_client = SimpleActionClient("/hand_controller/follow_joint_trajectory", FollowJointTrajectoryAction) self.__arm_traj_client = SimpleActionClient("/arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) if self.__hand_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False: rospy.logfatal("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.") raise Exception("Failed to connect to /hand_controller/follow_joint_trajectory in 4sec.") if self.__arm_traj_client.wait_for_server(timeout=rospy.Duration(4.0)) is False: rospy.logfatal("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.") raise Exception("Failed to connect to /arm_controller/follow_joint_trajectory in 4sec.") self.reset_world() def reset_world(self): """ Resets the object poses in the world and the robot joint angles. """ self.__switch_ctrl.call(start_controllers=[], stop_controllers=["hand_controller", "arm_controller", "joint_state_controller"], strictness=SwitchControllerRequest.BEST_EFFORT) self.__pause_physics.call() joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint', 'H1_F1J1', 'H1_F1J2', 'H1_F1J3', 'H1_F2J1', 'H1_F2J2', 'H1_F2J3', 'H1_F3J1', 'H1_F3J2', 'H1_F3J3'] joint_positions = [1.2, 0.3, -1.5, -0.5, -1.5, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0, 0.0, -0.3, 0.0] self.__set_model.call(model_name="smart_grasping_sandbox", urdf_param_name="robot_description", joint_names=joint_names, joint_positions=joint_positions) timer = Timer(0.0, self.__start_ctrl) timer.start() time.sleep(0.1) self.__unpause_physics.call() self.__reset_world.call() def get_object_pose(self): """ Gets the pose of the ball in the world frame. @return The pose of the ball. """ return self.__get_pose_srv.call(self.__current_model_name, "world").pose def get_tip_pose(self): """ Gets the current pose of the robot's tooltip in the world frame. @return the tip pose """ return self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose def move_tip_absolute(self, target): """ Moves the tooltip to the absolute target in the world frame @param target is a geometry_msgs.msg.Pose @return True on success """ self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([target]) plan = self.arm_commander.plan() if not self.arm_commander.execute(plan): return False return True def move_tip(self, x=0., y=0., z=0., roll=0., pitch=0., yaw=0.): """ Moves the tooltip in the world frame by the given x,y,z / roll,pitch,yaw. @return True on success """ transform = PyKDL.Frame(PyKDL.Rotation.RPY(pitch, roll, yaw), PyKDL.Vector(-x, -y, -z)) tip_pose = self.get_tip_pose() tip_pose_kdl = posemath.fromMsg(tip_pose) final_pose = toMsg(tip_pose_kdl * transform) self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([final_pose]) plan = self.arm_commander.plan() if not self.arm_commander.execute(plan): return False return True def send_command(self, command, duration=0.2): """ Send a dictionnary of joint targets to the arm and hand directly. @param command: a dictionnary of joint names associated with a target: {"H1_F1J1": -1.0, "shoulder_pan_joint": 1.0} @param duration: the amount of time it will take to get there in seconds. Needs to be bigger than 0.0 """ hand_goal = None arm_goal = None for joint, target in command.items(): if "H1" in joint: if not hand_goal: hand_goal = FollowJointTrajectoryGoal() point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(duration) hand_goal.trajectory.points.append(point) hand_goal.trajectory.joint_names.append(joint) hand_goal.trajectory.points[0].positions.append(target) else: if not arm_goal: arm_goal = FollowJointTrajectoryGoal() point = JointTrajectoryPoint() point.time_from_start = rospy.Duration.from_sec(duration) arm_goal.trajectory.points.append(point) arm_goal.trajectory.joint_names.append(joint) arm_goal.trajectory.points[0].positions.append(target) if arm_goal: self.__arm_traj_client.send_goal_and_wait(arm_goal) if hand_goal: self.__hand_traj_client.send_goal_and_wait(hand_goal) def get_current_joint_state(self): """ Gets the current state of the robot. @return joint positions, velocity and efforts as three dictionnaries """ joints_position = {n: p for n, p in zip(self.__last_joint_state.name, self.__last_joint_state.position)} joints_velocity = {n: v for n, v in zip(self.__last_joint_state.name, self.__last_joint_state.velocity)} joints_effort = {n: v for n, v in zip(self.__last_joint_state.name, self.__last_joint_state.effort)} return joints_position, joints_velocity, joints_effort def open_hand(self): """ Opens the hand. @return True on success """ self.hand_commander.set_named_target("open") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False return True def close_hand(self): """ Closes the hand. @return True on success """ self.hand_commander.set_named_target("close") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False return True def check_fingers_collisions(self, enable=True): """ Disables or enables the collisions check between the fingers and the objects / table @param enable: set to True to enable / False to disable @return True on success """ objects = ["cricket_ball__link", "drill__link", "cafe_table__link"] while self.__pub_planning_scene.get_num_connections() < 1: rospy.loginfo("waiting for someone to subscribe to the /planning_scene") rospy.sleep(0.1) request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = self.__get_planning_scene(request) acm = response.scene.allowed_collision_matrix for object_name in objects: if object_name not in acm.entry_names: # add object to allowed collision matrix acm.entry_names += [object_name] for row in range(len(acm.entry_values)): acm.entry_values[row].enabled += [False] new_row = deepcopy(acm.entry_values[0]) acm.entry_values += {new_row} for index_entry_values, entry_values in enumerate(acm.entry_values): if "H1_F" in acm.entry_names[index_entry_values]: for index_value, _ in enumerate(entry_values.enabled): if acm.entry_names[index_value] in objects: if enable: acm.entry_values[index_entry_values].enabled[index_value] = False else: acm.entry_values[index_entry_values].enabled[index_value] = True elif acm.entry_names[index_entry_values] in objects: for index_value, _ in enumerate(entry_values.enabled): if "H1_F" in acm.entry_names[index_value]: if enable: acm.entry_values[index_entry_values].enabled[index_value] = False else: acm.entry_values[index_entry_values].enabled[index_value] = True planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) self.__pub_planning_scene.publish(planning_scene_diff) rospy.sleep(1.0) return True def pick(self): """ Does its best to pick the ball. """ rospy.loginfo("Moving to Pregrasp") self.open_hand() time.sleep(0.1) ball_pose = self.get_object_pose() ball_pose.position.z += 0.5 #setting an absolute orientation (from the top) quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) ball_pose.orientation.x = quaternion[0] ball_pose.orientation.y = quaternion[1] ball_pose.orientation.z = quaternion[2] ball_pose.orientation.w = quaternion[3] self.move_tip_absolute(ball_pose) time.sleep(0.1) rospy.loginfo("Grasping") self.move_tip(y=-0.164) time.sleep(0.1) self.check_fingers_collisions(False) time.sleep(0.1) self.close_hand() time.sleep(0.1) rospy.loginfo("Lifting") for _ in range(5): self.move_tip(y=0.01) time.sleep(0.1) self.check_fingers_collisions(True) def swap_object(self, new_model_name): """ Replaces the current object with a new one.Replaces @new_model_name the name of the folder in which the object is (e.g. beer) """ try: self.__delete_model(self.__current_model_name) except: rospy.logwarn("Failed to delete: " + self.__current_model_name) try: sdf = None initial_pose = Pose() initial_pose.position.x = 0.15 initial_pose.position.z = 0.82 with open(self.__path_to_models + new_model_name + "/model.sdf", "r") as model: sdf = model.read() res = self.__spawn_model(new_model_name, sdf, "", initial_pose, "world") rospy.logerr( "RES: " + str(res) ) self.__current_model_name = new_model_name except: rospy.logwarn("Failed to delete: " + self.__current_model_name) def __compute_arm_target_for_ball(self): ball_pose = self.get_object_pose() # come at it from the top arm_target = ball_pose arm_target.position.z += 0.5 quaternion = quaternion_from_euler(-pi/2., 0.0, 0.0) arm_target.orientation.x = quaternion[0] arm_target.orientation.y = quaternion[1] arm_target.orientation.z = quaternion[2] arm_target.orientation.w = quaternion[3] return arm_target def __pre_grasp(self, arm_target): self.hand_commander.set_named_target("open") plan = self.hand_commander.plan() self.hand_commander.execute(plan, wait=True) for _ in range(10): self.arm_commander.set_start_state_to_current_state() self.arm_commander.set_pose_targets([arm_target]) plan = self.arm_commander.plan() if self.arm_commander.execute(plan): return True def __grasp(self, arm_target): waypoints = [] waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose) arm_above_ball = deepcopy(arm_target) arm_above_ball.position.z -= 0.12 waypoints.append(arm_above_ball) self.arm_commander.set_start_state_to_current_state() (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0) print fraction if not self.arm_commander.execute(plan): return False self.hand_commander.set_named_target("close") plan = self.hand_commander.plan() if not self.hand_commander.execute(plan, wait=True): return False self.hand_commander.attach_object("cricket_ball__link") def __lift(self, arm_target): waypoints = [] waypoints.append(self.arm_commander.get_current_pose(self.arm_commander.get_end_effector_link()).pose) arm_above_ball = deepcopy(arm_target) arm_above_ball.position.z += 0.1 waypoints.append(arm_above_ball) self.arm_commander.set_start_state_to_current_state() (plan, fraction) = self.arm_commander.compute_cartesian_path(waypoints, 0.01, 0.0) print fraction if not self.arm_commander.execute(plan): return False def __start_ctrl(self): rospy.loginfo("STARTING CONTROLLERS") self.__switch_ctrl.call(start_controllers=["hand_controller", "arm_controller", "joint_state_controller"], stop_controllers=[], strictness=1) def __joint_state_cb(self, msg): self.__last_joint_state = msg
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 hand.go() # load grasps
class ArmTracker: def __init__(self): rospy.init_node('arm_tracker') rospy.on_shutdown(self.shutdown) # Maximum distance of the target before the arm will lower self.max_target_dist = 1.2 # Arm length to center of gripper frame self.arm_length = 0.4 # Distance between the last target and the new target before we move the arm self.last_target_threshold = 0.01 # Distance between target and end-effector before we move the arm self.target_ee_threshold = 0.025 # Initialize the move group for the right arm self.right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Set the reference frame for pose targets self.reference_frame = REFERENCE_FRAME # Keep track of the last target pose self.last_target_pose = PoseStamped() # Set the right arm reference frame accordingly self.right_arm.set_pose_reference_frame(self.reference_frame) # Allow replanning to increase the chances of a solution self.right_arm.allow_replanning(False) # Set a position tolerance in meters self.right_arm.set_goal_position_tolerance(0.05) # Set an orientation tolerance in radians self.right_arm.set_goal_orientation_tolerance(0.2) # What is the end effector link? self.ee_link = self.right_arm.get_end_effector_link() # Create the transform listener self.listener = tf.TransformListener() # Queue up some tf data... rospy.sleep(3) # Set the gripper target to closed position using a joint value target right_gripper.set_joint_value_target(GRIPPER_CLOSED) # Plan and execute the gripper motion right_gripper.go() rospy.sleep(1) # Subscribe to the target topic rospy.wait_for_message('/target_pose', PoseStamped) # Use queue_size=1 so we don't pile up outdated target messages self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1) rospy.loginfo("Ready for action!") while not rospy.is_shutdown(): try: target = self.target except: rospy.sleep(0.5) continue # Timestamp the target with the current time target.header.stamp = rospy.Time() # Get the target pose in the right_arm shoulder lift frame #target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target) target_arm = self.listener.transformPose('right_arm_base_link', target) # Convert the position values to a Python list p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z] # Compute the distance between the target and the shoulder link dist_target_shoulder = euclidean(p0, [0, 0, 0]) # If the target is too far away, then lower the arm if dist_target_shoulder > self.max_target_dist: rospy.loginfo("Target is too far away") self.right_arm.set_named_target('resting') self.right_arm.go() rospy.sleep(1) continue # Transform the pose to the base reference frame target_base = self.listener.transformPose(self.reference_frame, target) # Compute the distance between the current target and the last target p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z] p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z] dist_last_target = euclidean(p1, p2) # Move the arm only if we are far enough away from the previous target if dist_last_target < self.last_target_threshold: rospy.loginfo("Still close to last target") rospy.sleep(0.5) continue # Get the pose of the end effector in the base reference frame ee_pose = self.right_arm.get_current_pose(self.ee_link) # Convert the position values to a Python list p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z] # Compute the distance between the target and the end-effector dist_target = euclidean(p1, p3) # Only move the arm if we are far enough away from the target if dist_target < self.target_ee_threshold: rospy.loginfo("Already close enough to target") rospy.sleep(1) continue # We want the gripper somewhere on the line connecting the shoulder and the target. # Using a parametric form of the line, the parameter ranges from 0 to the # minimum of the arm length and the distance to the target. t_max = min(self.arm_length, dist_target_shoulder) # Bring it back 10% so we don't collide with the target t = 0.9 * t_max # Now compute the target positions from the parameter try: target_arm.pose.position.x *= (t / dist_target_shoulder) target_arm.pose.position.y *= (t / dist_target_shoulder) target_arm.pose.position.z *= (t / dist_target_shoulder) except: rospy.sleep(1) rospy.loginfo("Exception!") continue # Transform to the base_footprint frame target_ee = self.listener.transformPose(self.reference_frame, target_arm) # Set the target gripper orientation to be horizontal target_ee.pose.orientation.x = 0 target_ee.pose.orientation.y = 0 target_ee.pose.orientation.z = 0 target_ee.pose.orientation.w = 1 # Update the current start state self.right_arm.set_start_state_to_current_state() # Set the target pose for the end-effector self.right_arm.set_pose_target(target_ee, self.ee_link) # Plan and execute the trajectory success = self.right_arm.go() if success: # Store the current target as the last target self.last_target_pose = target # Pause a bit between motions to keep from locking up rospy.sleep(0.5) def update_target_pose(self, target): self.target = target def relax_all_servos(self): command = 'rosrun donaxi_dynamixels arbotix_relax_all_servos.py' args = shlex.split(command) subprocess.Popen(args) def shutdown(self): # Stop any further target messages from being processed self.target_subscriber.unregister() # Stop any current arm movement self.right_arm.stop() # Move to the resting position self.right_arm.set_named_target('resting') self.right_arm.go() # Relax the servos self.relax_all_servos() os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the move group for the left arm left_arm = MoveGroupCommander('left_arm') right_arm.set_planner_id("KPIECEkConfigDefault"); left_arm.set_planner_id("KPIECEkConfigDefault"); rospy.sleep(1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the reference frame for pose targets reference_frame = 'base_footprint' # Set the right arm reference frame accordingly right_arm.set_pose_reference_frame(reference_frame) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file #left_arm.set_named_target('left_start') #left_arm.go() # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.56 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.51 box1_pose.pose.position.y = -0.1 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.49 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange 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.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.4 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # Set the target pose for the arm right_arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) right_arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(60) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "grasp" pose stored in the SRDF file right_arm.set_named_target('right_arm_up') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_OPEN) right_gripper.go() rospy.sleep(5) # Set the height of the table off the ground table_ground = 0.04 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] target_x = 0.135 #target_y = -0.32 target_y = -0.285290879999 # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.25 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = target_x target_pose.pose.position.y = target_y target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table blue and the boxes orange self.setColor(table_id, 0, 0, 0.8, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.18 place_pose.pose.position.y = 0 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 p = PoseStamped() p.header.frame_id = "up1_footprint" p.pose.position.x = 0.12792118579 p.pose.position.y = -0.285290879999 p.pose.position.z = 0.120301181892 p.pose.orientation.x = 0.0 p.pose.orientation.y = 0.0 p.pose.orientation.z = -0.706825181105 p.pose.orientation.w = 0.707388269167 right_gripper.set_pose_target(p.pose) # pick an object right_arm.allow_replanning(True) right_arm.allow_looking(True) right_arm.set_goal_tolerance(0.05) right_arm.set_planning_time(60) print "arm grasp" success = 0 attempt = 0 while not success: p_plan = right_arm.plan() attempt = attempt + 1 print "Planning attempt: " + str(attempt) if p_plan.joint_trajectory.points != []: success = 1 print "arm grasp" right_arm.execute(p_plan) rospy.sleep(5) right_gripper.set_joint_value_target(GRIPPER_GRASP) right_gripper.go() print "gripper closed" rospy.sleep(5) scene.attach_box(GRIPPER_FRAME, target_id) print "object attached" right_arm.set_named_target('right_arm_up') right_arm.go() print "arm up" rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) cartesian = rospy.get_param('~cartesian', True) # Connect to the right_arm move group right_arm = MoveGroupCommander('right_arm') # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame('base_footprint') # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "straight_forward" configuration stored in the SRDF file right_arm.set_named_target('straight_forward') # Plan and execute a trajectory to the goal configuration right_arm.go() # Get the current pose so we can add it as a waypoint start_pose = right_arm.get_current_pose(end_effector_link).pose # Initialize the waypoints list waypoints = [] # Set the first waypoint to be the starting pose if cartesian: # Append the pose to the waypoints list waypoints.append(start_pose) wpose = deepcopy(start_pose) # Set the next waypoint back 0.2 meters and right 0.2 meters wpose.position.x -= 0.2 wpose.position.y -= 0.2 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: right_arm.set_pose_target(wpose) right_arm.go() rospy.sleep(1) # Set the next waypoint to the right 0.15 meters wpose.position.x += 0.05 wpose.position.y += 0.15 wpose.position.z -= 0.15 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: right_arm.set_pose_target(wpose) right_arm.go() rospy.sleep(1) if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(start_pose)) else: right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(1) if cartesian: fraction = 0.0 maxtries = 100 attempts = 0 # Set the internal state to the current state right_arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < maxtries: (plan, fraction) = right_arm.compute_cartesian_path ( waypoints, # waypoint poses 0.01, # eef_step 0.0, # jump_threshold True) # avoid_collisions # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") right_arm.execute(plan) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # Move normally back to the 'resting' position right_arm.set_named_target('resting') right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
class DaArmServer: """The basic, design problem/tui agnostic arm server """ gestures = {} pointing_height = 0.06 grasp_height = 0.05 drop_height = 0.07 cruising_height = 0.1 START_TOLERANCE = 0.05 # this is for moveit to check for change in joint angles before moving GOAL_TOLERANCE = 0.005 moving = False paused = False move_group_state = "IDLE" last_joint_trajectory_goal = "" last_joint_trajectory_result = "" def __init__(self, num_planning_attempts=100, safe_zone=None): rospy.init_node("daarm_server", anonymous=True) self.safe_zone = safe_zone # this is a fallback zone to drop a block on fail if nothing is passed: [{x,y},{xT,yT}] self.init_params() self.init_scene() self.init_publishers() self.init_subscribers() self.init_action_clients() self.init_service_clients() self.init_arm(num_planning_attempts) def init_arm(self, num_planning_attempts=700): rospy.set_param( "/move_group/trajectory_execution/allowed_start_tolerance", self.START_TOLERANCE) self.arm = MoveGroupCommander("arm") self.gripper = MoveGroupCommander("gripper") self.robot = RobotCommander() self.arm.set_num_planning_attempts(num_planning_attempts) self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE) self.arm.set_goal_orientation_tolerance(0.02) self.init_services() self.init_action_servers() def init_scene(self): world_objects = [ "table", "tui", "monitor", "overHead", "wall", "farWall", "frontWall", "backWall", "blockProtector", "rearCamera" ] self.robot = RobotCommander() self.scene = PlanningSceneInterface() for obj in world_objects: self.scene.remove_world_object(obj) rospy.sleep(0.5) self.tuiPose = PoseStamped() self.tuiPose.header.frame_id = self.robot.get_planning_frame() self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51) self.tuiDimension = (0.9906, 0.8382, 0.8836) self.overHeadPose = PoseStamped() self.overHeadPose.header.frame_id = self.robot.get_planning_frame() self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97) self.overHeadDimension = (0.9906, 0.8382, 0.05) self.blockProtectorPose = PoseStamped() self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame( ) self.blockProtectorPose.pose.position = Point( 0.0056, -0.343, -0.51 + self.cruising_height) self.blockProtectorDimension = (0.9906, 0.8382, 0.8636) self.wallPose = PoseStamped() self.wallPose.header.frame_id = self.robot.get_planning_frame() self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048) self.wallDimension = (0.6096, 2, 1.35) self.farWallPose = PoseStamped() self.farWallPose.header.frame_id = self.robot.get_planning_frame() self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048) self.farWallDimension = (0.6096, 2, 3.35) self.frontWallPose = PoseStamped() self.frontWallPose.header.frame_id = self.robot.get_planning_frame() self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51) self.frontWallDimension = (1, 0.15, 4) self.backWallPose = PoseStamped() self.backWallPose.header.frame_id = self.robot.get_planning_frame() self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51) self.backWallDimension = (1, 0.005, 4) self.rearCameraPose = PoseStamped() self.rearCameraPose.header.frame_id = self.robot.get_planning_frame() self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51) self.rearCameraDimension = (0.5, 0.5, 2) rospy.sleep(0.5) self.scene.add_box("tui", self.tuiPose, self.tuiDimension) self.scene.add_box("wall", self.wallPose, self.wallDimension) self.scene.add_box("farWall", self.farWallPose, self.farWallDimension) self.scene.add_box("overHead", self.overHeadPose, self.overHeadDimension) self.scene.add_box("backWall", self.backWallPose, self.backWallDimension) self.scene.add_box("frontWall", self.frontWallPose, self.frontWallDimension) self.scene.add_box("rearCamera", self.rearCameraPose, self.rearCameraDimension) def raise_table(self): #raises the table obstacle to protect blocks on the table during transport self.scene.remove_world_object("blockProtector") self.scene.add_box("blockProtector", self.blockProtectorPose, self.blockProtectorDimension) def lower_table(self): #lowers the table to allow grasping into it self.scene.remove_world_object("blockProtector") def init_params(self): try: self.grasp_height = get_ros_param( "GRASP_HEIGHT", "Grasp height defaulting to 0.01") self.drop_height = get_ros_param("DROP_HEIGHT", "Drop height defaulting to 0.07") self.cruising_height = get_ros_param( "CRUISING_HEIGHT", "Cruising height defaulting to 0.1") self.pointing_height = get_ros_param( "POINT_HEIGHT", "Pointing height defaulting to 0.06") except ValueError as e: rospy.loginfo(e) def handle_param_update(self, message): self.init_params() def init_publishers(self): self.calibration_publisher = rospy.Publisher("/calibration_results", CalibrationParams, queue_size=1) self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs", String, queue_size=1) rospy.sleep(0.5) def init_subscribers(self): self.joint_angle_subscriber = rospy.Subscriber( '/j2s7s300_driver/out/joint_angles', JointAngles, self.update_joints) self.joint_trajectory_subscriber = rospy.Subscriber( '/j2s7s300/follow_joint_trajectory/status', GoalStatusArray, self.update_joint_trajectory_state) self.joint_trajectory_goal_subscriber = rospy.Subscriber( '/j2s7s300/follow_joint_trajectory/goal', FollowJointTrajectoryActionGoal, self.update_joint_trajectory_goal) self.joint_trajectory_result_subscriber = rospy.Subscriber( '/j2s7s300/follow_joint_trajectory/result', FollowJointTrajectoryActionResult, self.update_joint_trajectory_result) self.finger_position_subscriber = rospy.Subscriber( '/j2s7s300_driver/out/finger_position', FingerPosition, self.update_finger_position) self.param_update_subscriber = rospy.Subscriber( "/param_update", String, self.handle_param_update) self.moveit_status_subscriber = rospy.Subscriber( '/move_group/status', GoalStatusArray, self.update_move_group_status) self.move_it_feedback_subscriber = rospy.Subscriber( '/move_group/feedback', MoveGroupActionFeedback, self.update_move_group_state) #Topic for getting joint torques rospy.Subscriber('/j2s7s300_driver/out/joint_torques', JointAngles, self.monitorJointTorques) #Topic for getting cartesian force on end effector rospy.Subscriber('/j2s7s300_driver/out/tool_wrench', geometry_msgs.msg.WrenchStamped, self.monitorToolWrench) def init_action_servers(self): self.calibration_server = actionlib.SimpleActionServer( "calibrate_arm", CalibrateAction, self.calibrate, auto_start=False) self.calibration_server.start() self.move_block_server = actionlib.SimpleActionServer( "move_block", MoveBlockAction, self.handle_move_block, auto_start=False) self.move_block_server.start() #self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm) self.move_pose_server = actionlib.SimpleActionServer( "move_pose", MovePoseAction, self.handle_move_pose, auto_start=False) self.move_pose_server.start() def init_services(self): self.home_arm_service = rospy.Service("/home_arm", ArmCommand, self.handle_home_arm) # emergency stop self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand, self.handle_stop_arm) # stop and pause for a bit self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand, self.handle_pause_arm) self.start_arm_service = rospy.Service("/restart_arm", ArmCommand, self.handle_restart_arm) def init_action_clients(self): # Action Client for joint control joint_action_address = '/j2s7s300_driver/joints_action/joint_angles' self.joint_action_client = actionlib.SimpleActionClient( joint_action_address, kinova_msgs.msg.ArmJointAnglesAction) rospy.loginfo('Waiting for ArmJointAnglesAction server...') self.joint_action_client.wait_for_server() rospy.loginfo('ArmJointAnglesAction Server Connected') # Service to move the gripper fingers finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions' self.finger_action_client = actionlib.SimpleActionClient( finger_action_address, kinova_msgs.msg.SetFingersPositionAction) self.finger_action_client.wait_for_server() # def init_service_clients(self): self.is_simulation = False try: self.is_simulation = get_ros_param("IS_SIMULATION", "") except: self.is_simulation = False if self.is_simulation is True: # setup alternatives to jaco services for emergency stop, joint control, and finger control pass # Service to get TUI State rospy.wait_for_service('get_tui_blocks') self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState) # Service for homing the arm home_arm_service = '/j2s7s300_driver/in/home_arm' self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm) rospy.loginfo('Waiting for kinova home arm service') rospy.wait_for_service(home_arm_service) rospy.loginfo('Kinova home arm service server connected') # Service for emergency stop emergency_service = '/j2s7s300_driver/in/stop' self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop) rospy.loginfo('Waiting for Stop service') rospy.wait_for_service(emergency_service) rospy.loginfo('Stop service server connected') # Service for restarting the arm start_service = '/j2s7s300_driver/in/start' self.restart_arm = rospy.ServiceProxy(start_service, Start) rospy.loginfo('Waiting for Start service') rospy.wait_for_service(start_service) rospy.loginfo('Start service server connected') def handle_start_arm(self, message): return self.restart_arm() def handle_stop_arm(self, message): return self.stop_motion() def handle_pause_arm(self, message): self.stop_motion(home=True, pause=True) return str(self.paused) def handle_restart_arm(self, message): self.restart_arm() self.paused = False return str(self.paused) def handle_home_arm(self, message): try: status = self.home_arm() return json.dumps(status) except rospy.ServiceException as e: rospy.loginfo("Homing arm failed") def home_arm(self): # send the arm home # for now, let's just use the kinova home #self.home_arm_client() self.home_arm_kinova() return "done" def custom_home_arm(self): angles_set = [ 629.776062012, 150.076568694, -0.13603515923, 29.8505859375, 0.172727271914, 212.423721313, 539.743164062 ] goal = kinova_msgs.msg.ArmJointAnglesGoal() goal.angles.joint1 = angles_set[0] goal.angles.joint2 = angles_set[1] goal.angles.joint3 = angles_set[2] goal.angles.joint4 = angles_set[3] goal.angles.joint5 = angles_set[4] goal.angles.joint6 = angles_set[5] goal.angles.joint7 = angles_set[6] self.joint_action_client.send_goal(goal) def home_arm_kinova(self): """Takes the arm to the kinova default home if possible """ # self.arm.set_named_target("Home") angles_set = map(np.deg2rad, [ 629.776062012, 150.076568694, -0.13603515923, 29.8505859375, 0.172727271914, 212.423721313, 269.743164062 ]) self.arm.clear_pose_targets() try: self.arm.set_joint_value_target(angles_set) except MoveItCommanderException as e: pass #stupid bug in movegroupcommander wrapper throws an exception when trying to set joint angles try: self.arm.go() return "successful home" except: return "failed to home" # This callback function monitors the Joint Torques and stops the current execution if the Joint Torques exceed certain value def monitorJointTorques(self, torques): if abs(torques.joint1) > 1: return #self.emergency_stop() #Stop arm driver #rospy.sleep(1.0) #self.group.stop() #Stop moveit execution # This callback function monitors the Joint Wrench and stops the current # execution if the Joint Wrench exceeds certain value def monitorToolWrench(self, wrenchStamped): return #toolwrench = abs(wrenchStamped.wrench.force.x**2 + wrenchStamped.wrench.force.y**2 + wrenchStamped.wrench.force.z**2) ##print toolwrench #if toolwrench > 100: # self.emergency_stop() # Stop arm driver def move_fingers(self, finger1_pct, finger2_pct, finger3_pct): finger_max_turn = 6800 goal = kinova_msgs.msg.SetFingersPositionGoal() goal.fingers.finger1 = float((finger1_pct / 100.0) * finger_max_turn) goal.fingers.finger2 = float((finger2_pct / 100.0) * finger_max_turn) goal.fingers.finger3 = float((finger3_pct / 100.0) * finger_max_turn) self.finger_action_client.send_goal(goal) if self.finger_action_client.wait_for_result(rospy.Duration(5.0)): return self.finger_action_client.get_result() else: self.finger_action_client.cancel_all_goals() rospy.loginfo('the gripper action timed-out') return None def move_joint_angles(self, angle_set): goal = kinova_msgs.msg.ArmJointAnglesGoal() goal.angles.joint1 = angle_set[0] goal.angles.joint2 = angle_set[1] goal.angles.joint3 = angle_set[2] goal.angles.joint4 = angle_set[3] goal.angles.joint5 = angle_set[4] goal.angles.joint6 = angle_set[5] goal.angles.joint7 = angle_set[6] self.joint_action_client.send_goal(goal) if self.joint_action_client.wait_for_result(rospy.Duration(20.0)): return self.joint_action_client.get_result() else: print(' the joint angle action timed-out') self.joint_action_client.cancel_all_goals() return None def handle_move_block(self, message): """msg format: {id: int, source: Point {x: float,y: float}, target: Point {x: float, y: float} """ print(message) pick_x = message.source.x pick_y = message.source.y pick_x_threshold = message.source_x_tolerance pick_y_threshold = message.source_y_tolerance block_id = message.id place_x = message.target.x place_y = message.target.y place_x_threshold = message.target_x_tolerance place_y_threshold = message.target_y_tolerance self.move_block(block_id, pick_x, pick_y, pick_x_threshold, pick_y_threshold, place_x, place_y, place_x_threshold, place_y_threshold, message.block_size) def handle_pick_failure(self, exception): rospy.loginfo("Pick failed, going home.") self.open_gripper() self.home_arm() raise exception def handle_place_failure(self, safe_zone, block_size, exception): #should probably figure out if I'm holding the block first so it doesn't look weird #figure out how to drop the block somewhere safe #pass none and none if you are certain you haven't picked up a block yet if safe_zone is None and block_size is None: self.home_arm() raise exception rospy.loginfo("HANDLING PLACE FAILURE") block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response drop_location = self.calculate_drop_location( safe_zone[0]['x'], safe_zone[0]['y'], safe_zone[1]['x_tolerance'], safe_zone[1]['y_tolerance'], current_block_state, block_size, num_attempts=1000) try: arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y, get_tuio_bounds(), get_arm_bounds()) rospy.loginfo("panic arm drop: " + str(arm_drop_location)) self.place_block( Point(arm_drop_location[0], arm_drop_location[1], 0)) except Exception as e: rospy.loginfo( "ERROR: Cannot panic place the block! Get ready to catch it!") self.open_gripper() self.home_arm() raise exception def get_candidate_blocks(self, block_id, pick_x, pick_y, pick_x_tolerance, pick_y_tolerance): block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response candidate_blocks = [] print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance, pick_y_tolerance) # get candidate blocks -- blocks with the same id and within the error bounds/threshold given print(current_block_state) for block in current_block_state: print( block, self.check_block_bounds(block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance)) if block['id'] == block_id and self.check_block_bounds( block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance): candidate_blocks.append(block) return candidate_blocks def move_block(self, block_id, pick_x, pick_y, pick_x_tolerance, pick_y_tolerance, place_x, place_y, place_x_tolerance, place_y_tolerance, block_size=None, safe_zone=None, pick_tries=2, place_tries=1, point_at_block=True): if block_size is None: block_size = get_ros_param('DEFAULT_BLOCK_SIZE') block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response candidate_blocks = [] print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance, pick_y_tolerance) # get candidate blocks -- blocks with the same id and within the error bounds/threshold given print(current_block_state) # check for a drop location before trying to pick, do this before checking source to prevent cases where we go for a block user # moved while we are checking for a drop location drop_location = self.calculate_drop_location(place_x, place_y, place_x_tolerance, place_y_tolerance, current_block_state, block_size, num_attempts=2000) if drop_location == None: self.handle_place_failure( None, None, Exception("no room in the target zone to drop the block")) # here we are actually building a set of candidate blocks to pick for block in current_block_state: print( block, self.check_block_bounds(block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance)) if block['id'] == block_id and self.check_block_bounds( block['x'], block['y'], pick_x, pick_y, pick_x_tolerance, pick_y_tolerance): candidate_blocks.append(block) # select best block to pick and pick up pick_location = None if len(candidate_blocks) < 1: raise Exception("no block of id " + str(block_id) + " found within the source zone") elif len(candidate_blocks) == 1: pick_location = Point(candidate_blocks[0]['x'], candidate_blocks[0]['y'], 0) else: pick_location = Point(*self.find_most_isolated_block( candidate_blocks, current_block_state)) if point_at_block == True: try: arm_pick_location = tuio_to_arm(pick_location.x, pick_location.y, get_tuio_bounds(), get_arm_bounds()) arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y, get_tuio_bounds(), get_arm_bounds()) self.close_gripper() self.point_at_block(location=Point(arm_pick_location[0], arm_pick_location[1], 0)) self.point_at_block(location=Point(arm_drop_location[0], arm_drop_location[1], 0)) self.home_arm() except Exception as e: rospy.loginfo(str(e)) rospy.loginfo("failed trying to point at block. giving up.") self.home_arm() self.move_block_server.set_succeeded( MoveBlockResult(drop_location)) return for i in range(pick_tries): try: arm_pick_location = tuio_to_arm(pick_location.x, pick_location.y, get_tuio_bounds(), get_arm_bounds()) self.pick_block(location=Point(arm_pick_location[0], arm_pick_location[1], 0), check_grasp=True) break except Exception as e: if i < pick_tries - 1: rospy.loginfo("pick failed and trying again..." + str(e)) else: rospy.loginfo("pick failed and out of attempts..." + str(e)) self.handle_pick_failure(e) if safe_zone == None: if self.safe_zone == None: safe_zone = [{ 'x': pick_x, 'y': pick_y }, { 'x_tolerance': pick_x_tolerance, 'y_tolerance': pick_y_tolerance }] else: safe_zone = self.safe_zone # calculate drop location block_response = json.loads(self.get_block_state('tuio').tui_state) current_block_state = block_response drop_location = self.calculate_drop_location(place_x, place_y, place_x_tolerance, place_y_tolerance, current_block_state, block_size, num_attempts=2000) if drop_location == None: self.handle_place_failure( safe_zone, block_size, Exception("no room in the target zone to drop the block")) rospy.loginfo("tuio drop" + str(drop_location)) for i in range(place_tries): try: arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y, get_tuio_bounds(), get_arm_bounds()) rospy.loginfo("arm drop: " + str(arm_drop_location)) self.place_block( Point(arm_drop_location[0], arm_drop_location[1], 0)) break except Exception as e: if i < place_tries - 1: rospy.loginfo("place failed and trying again..." + str(e)) else: rospy.loginfo("place failed and out of attempts..." + str(e)) # check to see if we've defined a safe zone to drop the blocks self.handle_place_failure(safe_zone, block_size, e) # assume success if we made it this far self.move_block_server.set_succeeded(MoveBlockResult(drop_location)) # check if a certain x, y position is within the bounds of another x,y position @staticmethod def check_block_bounds(x, y, x_origin, y_origin, x_threshold, y_threshold): if x <= x_origin + x_threshold and x >= x_origin - x_threshold \ and y <= y_origin + y_threshold and y >= y_origin - y_threshold: return True return False # calculate the best location to drop the block def calculate_drop_location(self, x, y, x_threshold, y_threshold, blocks, block_size, num_attempts=1000): attempt = 0 x_original, y_original = x, y while (attempt < num_attempts): valid = True for block in blocks: if self.check_block_bounds(block['x'], block['y'], x, y, 0.8 * block_size, block_size): valid = False break if valid: return Point(x, y, 0) else: x = random.uniform(x_original - x_threshold, x_original + x_threshold) y = random.uniform(y_original - y_threshold, y_original + y_threshold) attempt += 1 #if none found in num_attempts, return none return None # candidates should have more than one element in it @staticmethod def find_most_isolated_block(candidates, all_blocks): print(candidates) min_distances = [] # tuples of candidate, distance to closest block for candidate in candidates: min_dist = -1 for block in all_blocks: if block['x'] == candidate['x'] and block['y'] == candidate[ 'y']: continue else: dist = DaArmServer.block_dist(candidate, block) if min_dist == -1 or dist < min_dist: min_dist = dist min_distances.append([candidate, min_dist]) most_isolated, _ = max(min_distances, key=lambda x: x[ 1]) # get most isolated candidate, and min_distance return most_isolated['x'], most_isolated['y'], 0 @staticmethod def block_dist(block_1, block_2): return np.sqrt((block_2['x'] - block_1['x'])**2 + (block_2['y'] - block_1['y'])**2) def update_finger_position(self, message): self.finger_positions = [ message.finger1, message.finger2, message.finger3 ] def check_grasp(self): closed_pos = 0.95 * 6800 distance_from_closed = 0 for fp in self.finger_positions: distance_from_closed += (closed_pos - fp)**2 if np.sqrt(distance_from_closed ) > 130: #this is just some magic number for now return True #if the fingers aren't fully closed, then grasp is good else: return False def open_gripper(self, delay=0): """open the gripper ALL THE WAY, then delay """ if self.is_simulation is True: self.gripper.set_named_target("Open") self.gripper.go() else: try: rospy.loginfo("Opening Gripper!!") self.move_fingers(50, 50, 50) except Exception as e: rospy.loginfo("Caught it!!" + str(e)) rospy.sleep(delay) def close_gripper(self, delay=0): """close the gripper ALL THE WAY, then delay """ # self.gripper.set_named_target("Close") # self.gripper.go() try: rospy.loginfo("Closing Gripper!!") self.move_fingers(95, 95, 95) except Exception as e: rospy.loginfo("Caught it!!" + str(e)) rospy.sleep(delay) def handle_gesture(self, gesture): # lookup the gesture from a table? or how about a message? # one possibility, object that contains desired deltas in pos/orientation # as well as specify the arm or gripper choice pass def handle_move_pose(self, message): # takes a geometry_msgs/Pose message self.move_arm_to_pose(message.target.position, message.target.orientation, action_server=self.move_pose_server) self.move_pose_server.set_succeeded() def check_plan_result(self, target_pose, cur_pose): #we'll do a very lenient check, this is to find failures, not error #also only checking position, not orientation rospy.loginfo("checking pose:" + str(target_pose) + str(cur_pose)) if np.abs(target_pose.pose.position.x - cur_pose.pose.position.x) > self.GOAL_TOLERANCE * 2: print("x error too far") return False if np.abs(target_pose.pose.position.y - cur_pose.pose.position.y) > self.GOAL_TOLERANCE * 2: print("y error too far") return False if np.abs(target_pose.pose.position.z - cur_pose.pose.position.z) > self.GOAL_TOLERANCE * 2: print("z error too far") return False print("tolerable error") return True # expects cooridinates for position to be in arm space def move_arm_to_pose(self, position, orientation, delay=0.5, waypoints=[], corrections=4, action_server=None): for i in range(corrections + 1): rospy.loginfo("TRY NUMBER " + str(i)) if len(waypoints) > 0 and i < 1: # this is good for doing gestures plan, fraction = self.arm.compute_cartesian_path( waypoints, eef_step=0.01, jump_threshold=0.0) else: p = self.arm.get_current_pose() p.pose.position = position p.pose.orientation = orientation rospy.loginfo("PLANNING TO " + str(p)) self.arm.set_pose_target(p) last_traj_goal = self.last_joint_trajectory_goal rospy.loginfo("EXECUTING!") plan = self.arm.go(wait=False) timeout = 5 / 0.001 while self.last_joint_trajectory_goal == last_traj_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Timeout waiting for kinova to accept movement goal." )) rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL") current_goal = self.last_joint_trajectory_goal # then loop until a result for it gets published timeout = 90 / 0.001 while self.last_joint_trajectory_result != current_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Motion took longer than 90 seconds. aborting...")) if self.paused is True: self.arm.stop() # cancel the moveit goals return rospy.loginfo("LEAVING THE WHILE LOOP") # for i in range(corrections): # rospy.loginfo("Correcting the pose") # self.move_joint_angles(angle_set) rospy.sleep(delay) if (self.check_plan_result(p, self.arm.get_current_pose())): break #we're there, no need to retry #rospy.loginfo("OK GOT THROUGH THE PLANNING PHASE") if False: # # get the last pose to correct if desired # ptPos = plan.joint_trajectory.points[-1].positions # # print "==================================" # # print "Last point of the current trajectory: " # angle_set = list() # for i in range(len(ptPos)): # tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360 # angle_set.append(tempPos) if action_server: pass #action_server.publish_feedback(CalibrateFeedback("Plan Found")) last_traj_goal = self.last_joint_trajectory_goal rospy.loginfo("EXECUTING!") self.arm.execute(plan, wait=False) # this is a bit naive, but I'm going to loop until a new trajectory goal gets published timeout = 5 / 0.001 while self.last_joint_trajectory_goal == last_traj_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Timeout waiting for kinova to accept movement goal." )) rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL") current_goal = self.last_joint_trajectory_goal # then loop until a result for it gets published timeout = 15 / 0.001 while self.last_joint_trajectory_result != current_goal: rospy.sleep(0.001) timeout -= 1 if timeout <= 0: raise (Exception( "Motion took longer than 15 seconds. aborting...")) if self.paused is True: self.arm.stop() # cancel the moveit goals return rospy.loginfo("LEAVING THE WHILE LOOP") # for i in range(corrections): # rospy.loginfo("Correcting the pose") # self.move_joint_angles(angle_set) rospy.sleep(delay) else: if action_server: #action_server.publish_feedback(CalibrateFeedback("Planning Failed")) pass # Let's have the caller handle sequences instead. # def handle_move_sequence(self, message): # # if the move fails, do we cancel the sequence # cancellable = message.cancellable # moves = message.moves # for move in moves: # try: # self.handle_move_block(move) # except Exception: # if cancellable: # rospy.loginfo("Part of move failed, cancelling the rest.") # break def update_move_group_state(self, message): rospy.loginfo(message.feedback.state) self.move_group_state = message.feedback.state def update_move_group_status(self, message): if message.status_list: #rospy.loginfo("MoveGroup Status for "+str(message.status_list[0].goal_id.id)+": "+str(message.status_list[0].status)) self.move_group_status = message.status_list[0].status def update_joint_trajectory_state(self, message): # print(message.status_list) if len(message.status_list) > 0: self.joint_trajectory_state = message.status_list[0].status else: self.joint_trajectory_state = 0 def update_joint_trajectory_goal(self, message): #print("goalisasis", message.goal_id.id) self.last_joint_trajectory_goal = message.goal_id.id def update_joint_trajectory_result(self, message): #print("resultisasis", message.status.goal_id.id) self.last_joint_trajectory_result = message.status.goal_id.id def get_grasp_orientation(self, position): #return Quaternion(0, 0, 1/np.sqrt(2), 1/np.sqrt(2)) return Quaternion(-0.707388, -0.706825, 0.0005629, 0.0005633) def update_joints(self, joints): self.joint_angles = [ joints.joint1, joints.joint2, joints.joint3, joints.joint4, joints.joint5, joints.joint6, joints.joint7 ] def move_z_relative(self, distance): p = self.arm.get_current_pose() p.pose.position.z += distance self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0) def move_z_absolute(self, height): p = self.arm.get_current_pose() p.pose.position.z = height self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0) def pick_block(self, location, check_grasp=False, retry_attempts=0, delay=0, action_server=None): # go to a spot and pick up a block # if check_grasp is true, it will check torque on gripper and retry or fail if not holding # open the gripper # print('Position: ', position) self.open_gripper() position = Point(location.x, location.y, self.cruising_height) orientation = self.get_grasp_orientation(position) try: self.raise_table() self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) self.lower_table() position = Point(location.x, location.y, self.grasp_height) self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) except Exception as e: current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < 0: self.move_z_absolute(self.crusing_height) self.raise_table() raise ( e ) #problem because sometimes we get exception e.g. if we're already there # and it will skip the close if so. if action_server: action_server.publish_feedback() self.close_gripper() self.move_z_absolute(self.cruising_height) #try to wait until we're up to check grasp rospy.sleep(0.5) if check_grasp is True: if (self.check_grasp() is False): print("Grasp failed!") self.raise_table() raise (Exception("grasp failed!")) # for now, but we want to check finger torques # for i in range(retry_attempts): # self.move_z(0.3) self.raise_table() rospy.sleep(delay) def place_block(self, location, check_grasp=False, delay=0, action_server=None): # go to a spot an drop a block # if check_grasp is true, it will check torque on gripper and fail if not holding a block # print('Position: ', position) current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct self.move_z_absolute(self.cruising_height) position = Point(location.x, location.y, self.cruising_height) rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " + str(self.drop_height)) orientation = self.get_grasp_orientation(position) try: self.raise_table( ) # only do this as a check in case it isn't already raised self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) self.lower_table() self.move_z_absolute(self.drop_height) except Exception as e: current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < 0: self.move_z_absolute(self.cruising_height) self.raise_table() raise (e) if action_server: action_server.publish_feedback() self.open_gripper() self.move_z_absolute(self.cruising_height) self.raise_table() self.close_gripper() def point_at_block(self, location, delay=0, action_server=None): # go to a spot an drop a block # if check_grasp is true, it will check torque on gripper and fail if not holding a block # print('Position: ', position) current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct self.move_z_absolute(self.cruising_height) position = Point(location.x, location.y, self.cruising_height) rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " + str(self.drop_height)) orientation = self.get_grasp_orientation(position) try: self.raise_table( ) # only do this as a check in case it isn't already raised self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server) self.lower_table() self.move_z_absolute(self.pointing_height) self.move_z_absolute(self.cruising_height) except Exception as e: current_pose = self.arm.get_current_pose() if current_pose.pose.position.z - self.cruising_height < 0: self.move_z_absolute(self.cruising_height) self.raise_table() raise (e) if action_server: action_server.publish_feedback() self.raise_table() def stop_motion(self, home=False, pause=False): rospy.loginfo("STOPPING the ARM") # cancel the moveit_trajectory # self.arm.stop() # call the emergency stop on kinova self.emergency_stop() # rospy.sleep(0.5) if pause is True: self.paused = True if home is True: # self.restart_arm() self.home_arm() def calibrate(self, message): print("calibrating ", message) self.place_calibration_block() rospy.sleep(5) # five seconds to correct the drop if it bounced, etc. print("moving on...") self.calibration_server.publish_feedback( CalibrateFeedback("getting the coordinates")) params = self.record_calibration_params() self.set_arm_calibration_params(params[0], params[1]) self.calibration_publisher.publish( CalibrationParams(params[0], params[1])) self.calibration_server.set_succeeded(CalibrateResult(params)) def set_arm_calibration_params(self, arm_x_min, arm_y_min): rospy.set_param("ARM_X_MIN", arm_x_min) rospy.set_param("ARM_Y_MIN", arm_y_min) def place_calibration_block(self): # start by homing the arm (kinova home) self.calibration_server.publish_feedback(CalibrateFeedback("homing")) self.home_arm_kinova() # go to grab a block from a human self.open_gripper() self.close_gripper() self.open_gripper(4) self.close_gripper(2) # move to a predetermined spot self.calibration_server.publish_feedback( CalibrateFeedback("moving to drop")) try: self.move_arm_to_pose(Point(0.4, -0.4, 0.1), Quaternion(1, 0, 0, 0), corrections=0) except Exception as e: rospy.loginfo("THIS IS TH PRoblem" + str(e)) # drop the block self.open_gripper() self.calibration_server.publish_feedback( CalibrateFeedback("dropped the block")) calibration_block = {'x': 0.4, 'y': -0.4, 'id': 0} calibration_action_belief = { "action": "add", "block": calibration_block } self.action_belief_publisher.publish( String(json.dumps(calibration_action_belief))) rospy.loginfo("published arm belief") return def record_calibration_params(self): """ Call the block_tracker service to get the current table config. Find the calibration block and set the appropriate params. """ # make sure we know the dimensions of the table before we start # fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y tuio_bounds = get_tuio_bounds() arm_bounds = get_arm_bounds(calibrate=False) try: block_state = json.loads(self.get_block_state("tuio").tui_state) except rospy.ServiceException as e: # self.calibration_server.set_aborted() raise (ValueError("Failed getting block state to calibrate: " + str(e))) if len(block_state) != 1: # self.calibration_server.set_aborted() raise (ValueError( "Failed calibration, either couldn't find block or > 1 block on TUI!" )) # if we made it here, let's continue! # start by comparing the reported tuio coords where we dropped the block # with the arm's localization of the end-effector that dropped it # (we assume that the block fell straight below the drop point) drop_pose = self.arm.get_current_pose() end_effector_x = drop_pose.pose.position.x end_effector_y = drop_pose.pose.position.y block_tuio_x = block_state[0]['x'] block_tuio_y = block_state[0]['y'] x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y, tuio_bounds) arm_x_min = end_effector_x - x_ratio * arm_bounds["x_dist"] arm_y_min = end_effector_y - y_ratio * arm_bounds["y_dist"] return [arm_x_min, arm_y_min]
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Pause for the scene to get ready rospy.sleep(1) # Initialize the MoveIt! commander for the right arm right_arm = MoveGroupCommander('right_arm') # Initialize the MoveIt! commander for the gripper right_gripper = MoveGroupCommander('right_gripper') # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.01) right_arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Allow 5 seconds per planning attempt right_arm.set_planning_time(5) # Remove leftover objects from a previous run scene.remove_attached_object(end_effector_link, 'tool') scene.remove_world_object('table') scene.remove_world_object('box1') scene.remove_world_object('box2') scene.remove_world_object('target') # Set the height of the table off the ground table_ground = 0.75 # Set the length, width and height of the table table_size = [0.2, 0.7, 0.01] # Set the length, width and height of the object to attach tool_size = [0.3, 0.02, 0.02] # Create a pose for the tool relative to the end-effector p = PoseStamped() p.header.frame_id = end_effector_link scene.attach_mesh # Place the end of the object within the grasp of the gripper p.pose.position.x = tool_size[0] / 2.0 - 0.025 p.pose.position.y = 0.0 p.pose.position.z = 0.0 # Align the object with the gripper (straight out) p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 # Attach the tool to the end-effector scene.attach_box(end_effector_link, 'tool', p, tool_size) # Add a floating table top table_pose = PoseStamped() table_pose.header.frame_id = 'base_footprint' table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box('table', table_pose, table_size) # Update the current state right_arm.set_start_state_to_current_state() # Move the arm with the attached object to the 'straight_forward' position right_arm.set_named_target('straight_forward') right_arm.go() rospy.sleep(2) # Return the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(2) scene.remove_attached_object(end_effector_link, 'tool') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
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=5) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('ur_arm') # 获取终端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_link' 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(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(2) # 设置桌面的高度 table_ground = 0.25 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = 0.26 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = 0.21 box1_pose.pose.position.y = -0.1 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.19 box2_pose.pose.position.y = 0.15 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 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.4, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置机械臂的运动目标位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = 0.2 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) # 设置机械臂的运动目标位置,进行避障规划 target_pose2 = PoseStamped() target_pose2.header.frame_id = reference_frame target_pose2.pose.position.x = 0.2 target_pose2.pose.position.y = -0.25 target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 target_pose2.pose.orientation.w = 1.0 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose2, end_effector_link) arm.go() rospy.sleep(2) # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def callback(pose): object_position_info = pose.position object_orientation_info = pose.orientation print object_position_info moveit_commander.roscpp_initialize(sys.argv) #rospy.init_node('moveit_cartesian', anonymous=True) cartesian = rospy.get_param('~cartesian', True) #set cartesian parameters ur5_manipulator = MoveGroupCommander('manipulator') ur5_gripper = MoveGroupCommander('gripper') ur5_manipulator.allow_replanning(True) ur5_manipulator.set_pose_reference_frame('base_link') ur5_manipulator.set_goal_position_tolerance(0.01) ur5_manipulator.set_goal_orientation_tolerance(0.1) end_effector_link = ur5_manipulator.get_end_effector_link() ur5_manipulator.set_named_target('home_j') ur5_manipulator.go() ur5_gripper.set_named_target('open') ur5_gripper.go() #get the end effort information start_pose = ur5_manipulator.get_current_pose(end_effector_link).pose print("The first waypoint:") print(start_pose) #define waypoints waypoints = [] waypoints.append(start_pose) wpose = deepcopy(start_pose) wpose.position.z = object_position_info.z+0.25 wpose.position.x = object_position_info.x wpose.position.y = object_position_info.y print("The second waypoint:") print(wpose) waypoints.append(deepcopy(wpose)) print(" ") print(waypoints) if cartesian: fraction = 0.0 maxtries = 100 attempts = 0 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = ur5_manipulator.compute_cartesian_path ( waypoints, 0.01, 0.0, True) attempts += 1 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") ur5_manipulator.execute(plan) rospy.sleep(2) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") rospy.sleep(3) ur5_gripper.set_named_target("close") plan = ur5_gripper.go() rospy.sleep(2) ur5_manipulator.set_named_target('home_j') ur5_manipulator.go() rospy.sleep(3) moveit_commander.roscpp_shutdown()
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_drawing', anonymous=True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.01) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(0.5) arm.set_max_velocity_scaling_factor(0.5) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) star_pose = PoseStamped() star_pose.header.frame_id = reference_frame star_pose.header.stamp = rospy.Time.now() star_pose.pose.position.x = 0.40 star_pose.pose.position.y = 0.0 star_pose.pose.position.z = 0.12 star_pose.pose.orientation.w = 1.0 # 设置机械臂终端运动的目标位姿 arm.set_pose_target(star_pose, end_effector_link) arm.go() radius = 0.1 centerY = 0.0 centerX = 0.40 - radius # 初始化路点列表 waypoints = [] starPoints = [] pose_temp = star_pose for th in numpy.arange(0, 6.2831855, 1.2566371): pose_temp.pose.position.y = -(centerY + radius * math.sin(th)) pose_temp.pose.position.x = centerX + radius * math.cos(th) pose_temp.pose.position.z = 0.113 wpose = deepcopy(pose_temp.pose) starPoints.append(deepcopy(wpose)) # 将圆弧上的路径点加入列表 waypoints.append(starPoints[0]) waypoints.append(starPoints[2]) waypoints.append(starPoints[4]) waypoints.append(starPoints[1]) waypoints.append(starPoints[3]) waypoints.append(starPoints[0]) fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点,完成圆弧轨迹 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_cartesian_demo', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('manipulator') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('base_link') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置每次运动规划的时间限制:1s arm.set_planning_time(1) # 控制机械臂运动到之前设置的“home”姿态 arm.set_named_target('home') arm.go() # 获取当前位姿数据为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose # --------------------- 第一段轨迹生成,关节空间插值 ---------------------# # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [0, -1, 1, 0, -1, 0] arm.set_joint_value_target(joint_positions) # 控制机械臂完成运动 arm.go() rospy.sleep(0.5) # 获取当前位姿数据为机械臂运动的起始位姿 start_pose = arm.get_current_pose(end_effector_link).pose # --------------------- 第一段轨迹生成,关节空间插值 ---------------------# # 初始化路点列表 waypoints = [] # 将初始位姿加入路点列表 if cartesian: waypoints.append(start_pose) # 设置第二个路点数据,并加入路点列表 # 第二个路点需要向后运动0.3米,向右运动0.3米 wpose = deepcopy(start_pose) wpose.orientation.x = 0 wpose.orientation.y = 0.707106781186547 wpose.orientation.z = 0 wpose.orientation.w = 0.707106781186547 wpose.position.x -= 0.2 # wpose.position.y -= 0.4 wpose.position.z -= 1 if cartesian: waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # 设置第三个路点数据,并加入路点列表 wpose.orientation.x = 0 wpose.orientation.y = 0.707106781186547 wpose.orientation.z = 0 wpose.orientation.w = 0.707106781186547 # wpose.position.x -= 0.5 wpose.position.y -= 0.5 wpose.position.z += 0.2 # wpose.position.x += 0.15 # wpose.position.y += 0.1 # wpose.position.z -= 0.15 if cartesian: waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # 设置第四个路点数据,回到初始位置,并加入路点列表 if cartesian: waypoints.append(deepcopy(start_pose)) else: arm.set_pose_target(start_pose) arm.go() rospy.sleep(1) if cartesian: fraction = 0.0 #路径规划覆盖率 maxtries = 100 #最大尝试规划次数 attempts = 0 #已经尝试规划次数 # 设置机器臂当前的状态作为运动初始状态 arm.set_start_state_to_current_state() # 尝试规划一条笛卡尔空间下的路径,依次通过所有路点 while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path ( waypoints, # waypoint poses,路点列表 0.01, # eef_step,终端步进值 0.0, # jump_threshold,跳跃阈值 True) # avoid_collisions,避障规划 # 尝试次数累加 attempts += 1 # 打印运动规划进程 if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # 如果路径规划成功(覆盖率100%),则开始控制机械臂运动 if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan) rospy.loginfo("Path execution complete.") # 如果路径规划失败,则打印失败信息 else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('TCP_Move', anonymous=True) # 是否需要使用笛卡尔空间的运动规划 cartesian = rospy.get_param('~cartesian', True) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('xarm6') # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame('link_base') # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.001) arm.set_goal_orientation_tolerance(0.001) # 设置允许的最大速度和加速度 arm.set_max_acceleration_scaling_factor(0.1) arm.set_max_velocity_scaling_factor(0.1) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 角度弧度转换 j1 = 90.0 / 180 * math.pi j2 = -18.6 / 180 * math.pi j3 = -28.1 / 180 * math.pi j4 = 1.0 / 180 * math.pi j5 = 47.6 / 180 * math.pi j6 = -0.9 / 180 * math.pi # 设置机械臂的目标位置,使用六轴的位置数据进行描述(单位:弧度) joint_positions = [j1, j2, j3, j4, j5, j6] arm.set_joint_value_target(joint_positions) arm.go() rospy.sleep(1) # 向下按压门把手 current_pose = arm.get_current_joint_values() current_pose[4] += (20.0 / 180.0) * math.pi arm.set_joint_value_target(current_pose) arm.go() rospy.sleep(1) #推开门 current_pose = arm.get_current_joint_values() current_pose[0] -= (42.0 / 180.0) * math.pi current_pose[1] += (2.0 / 180.0) * math.pi current_pose[2] -= (11.4 / 180.0) * math.pi arm.set_joint_value_target(current_pose) arm.go() rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_move_cartesian', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) # arm.set_goal_position_tolerance(0.01) arm.set_named_target(START_POSITION) arm.go() rospy.sleep(2) waypoints = list() pose = PoseStamped().pose # start with the current pose waypoints.append(copy.deepcopy(arm.get_current_pose(arm.get_end_effector_link()).pose)) # same orientation for all q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw pose.orientation = Quaternion(*q) # first pose pose.position.x = 0.35 pose.position.y = 0.25 pose.position.z = 0.3 waypoints.append(copy.deepcopy(pose)) # second pose pose.position.x = 0.25 pose.position.y = -0.3 pose.position.z = 0.3 waypoints.append(copy.deepcopy(pose)) # third pose pose.position.x += 0.15 waypoints.append(copy.deepcopy(pose)) # fourth pose pose.position.y += 0.15 waypoints.append(copy.deepcopy(pose)) (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold print "====== waypoints created =======" # print waypoints # ======= show cartesian path ====== # arm.go() rospy.sleep(10) print "========= end =========" moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
pass ### Create a handle for the Planning Scene Interface psi = PlanningSceneInterface() rospy.sleep(1.0) ### Create a handle for the Move Group Commander mgc = MoveGroupCommander("arm") rospy.sleep(1.0) ### Add virtual obstacle pose = gen_pose(pos=[-0.2, -0.1, 1.2]) psi.add_box("box", pose, size=(0.15, 0.15, 0.6)) rospy.sleep(1.0) ### Move to stored joint position mgc.set_named_target("left") mgc.go() ### Move to Cartesian position goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707]) mgc.go(goal_pose.pose) ### Move Cartesian linear goal_pose.pose.position.z -= 0.1 (traj, frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True) mgc.execute(traj) print "Done"
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 'adding scene objects' scene.add_mesh("bowl", p, "8inhemi.STL") scene.add_mesh("punch", p1, "punch.STL") scene.add_mesh("glovebox", p2, "GloveBox.stl") print 'attaching bowl...' robot.attach_object("bowl") rospy.sleep(2) currentbowlpose = p; gripper.set_named_target("pinch")
class SrRobotCommander(object): """ Base class for hand and arm commanders """ def __init__(self, name): """ Initialize MoveGroupCommander object @param name - name of the MoveIt group """ self._name = name self._move_group_commander = MoveGroupCommander(name) self._robot_commander = RobotCommander() self._robot_name = self._robot_commander._r.get_robot_name() self.refresh_named_targets() self._warehouse_name_get_srv = rospy.ServiceProxy( "get_robot_state", GetState) self._planning_scene = PlanningSceneInterface() self._joint_states_lock = threading.Lock() self._joint_states_listener = \ rospy.Subscriber("joint_states", JointState, self._joint_states_callback, queue_size=1) self._joints_position = {} self._joints_velocity = {} self._joints_effort = {} self._joints_state = None self._clients = {} self.__plan = None self._controllers = {} rospy.wait_for_service('compute_ik') self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) controller_list_param = rospy.get_param("/move_group/controller_list") # create dictionary with name of controllers and corresponding joints self._controllers = { item["name"]: item["joints"] for item in controller_list_param } self._set_up_action_client(self._controllers) self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) threading.Thread(None, rospy.spin) def set_planner_id(self, planner_id): self._move_group_commander.set_planner_id(planner_id) def set_num_planning_attempts(self, num_planning_attempts): self._move_group_commander.set_num_planning_attempts( num_planning_attempts) def set_planning_time(self, seconds): self._move_group_commander.set_planning_time(seconds) def get_end_effector_pose_from_named_state(self, name): state = self._warehouse_name_get_srv(name, self._robot_name).state return self.get_end_effector_pose_from_state(state) def get_end_effector_pose_from_state(self, state): header = Header() fk_link_names = [self._move_group_commander.get_end_effector_link()] header.frame_id = self._move_group_commander.get_pose_reference_frame() response = self._forward_k(header, fk_link_names, state) return response.pose_stamped[0] def get_planning_frame(self): return self._move_group_commander.get_planning_frame() def set_pose_reference_frame(self, reference_frame): self._move_group_commander.set_pose_reference_frame(reference_frame) def get_group_name(self): return self._name def refresh_named_targets(self): self._srdf_names = self.__get_srdf_names() self._warehouse_names = self.__get_warehouse_names() def set_max_velocity_scaling_factor(self, value): self._move_group_commander.set_max_velocity_scaling_factor(value) def set_max_acceleration_scaling_factor(self, value): self._move_group_commander.set_max_acceleration_scaling_factor(value) def allow_looking(self, value): self._move_group_commander.allow_looking(value) def allow_replanning(self, value): self._move_group_commander.allow_replanning(value) def execute(self): """ Executes the last plan made. """ if self.check_plan_is_valid(): self._move_group_commander.execute(self.__plan) self.__plan = None else: rospy.logwarn("No plans were made, not executing anything.") def execute_plan(self, plan): if self.check_given_plan_is_valid(plan): self._move_group_commander.execute(plan) self.__plan = None else: rospy.logwarn("Plan is not valid, not executing anything.") def move_to_joint_value_target(self, joint_states, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self._move_group_commander.go(wait=wait) def plan_to_joint_value_target(self, joint_states, angle_degrees=False): """ Set target of the robot's links and plans. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param angle_degrees - are joint_states in degrees or not This is a blocking method. """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self.__plan = self._move_group_commander.plan() return self.__plan def check_plan_is_valid(self): """ Checks if current plan contains a valid trajectory """ return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0) def check_given_plan_is_valid(self, plan): """ Checks if given plan contains a valid trajectory """ return (plan is not None and len(plan.joint_trajectory.points) > 0) def get_robot_name(self): return self._robot_name def named_target_in_srdf(self, name): return name in self._srdf_names def set_named_target(self, name): if name in self._srdf_names: self._move_group_commander.set_named_target(name) elif (name in self._warehouse_names): response = self._warehouse_name_get_srv(name, self._robot_name) active_names = self._move_group_commander._g.get_active_joints() joints = response.state.joint_state.name positions = response.state.joint_state.position js = {} for n, this_name in enumerate(joints): if this_name in active_names: js[this_name] = positions[n] self._move_group_commander.set_joint_value_target(js) else: rospy.logerr("Unknown named state '%s'..." % name) return False return True def get_named_target_joint_values(self, name): output = dict() if (name in self._srdf_names): output = self._move_group_commander.\ _g.get_named_target_values(str(name)) elif (name in self._warehouse_names): js = self._warehouse_name_get_srv( name, self._robot_name).state.joint_state for x, n in enumerate(js.name): if n in self._move_group_commander._g.get_joints(): output[n] = js.position[x] else: rospy.logerr("No target named %s" % name) return None return output def get_end_effector_link(self): return self._move_group_commander.get_end_effector_link() def get_current_pose(self, reference_frame=None): """ Get the current pose of the end effector. @param reference_frame - The desired reference frame in which end effector pose should be returned. If none is passed, it will use the planning frame as reference. @return geometry_msgs.msg.Pose() - current pose of the end effector """ if reference_frame is not None: try: trans = self.tf_buffer.lookup_transform( reference_frame, self._move_group_commander.get_end_effector_link(), rospy.Time(0), rospy.Duration(5.0)) current_pose = geometry_msgs.msg.Pose() current_pose.position.x = trans.transform.translation.x current_pose.position.y = trans.transform.translation.y current_pose.position.z = trans.transform.translation.z current_pose.orientation.x = trans.transform.rotation.x current_pose.orientation.y = trans.transform.rotation.y current_pose.orientation.z = trans.transform.rotation.z current_pose.orientation.w = trans.transform.rotation.w return current_pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logwarn( "Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() + " in " + reference_frame + " reference frame") return None else: return self._move_group_commander.get_current_pose().pose def get_current_state(self): """ Get the current joint state of the group being used. @return a dictionary with the joint names as keys and current joint values """ joint_names = self._move_group_commander._g.get_active_joints() joint_values = self._move_group_commander._g.get_current_joint_values() return dict(zip(joint_names, joint_values)) def get_current_state_bounded(self): """ Get the current joint state of the group being used, enforcing that they are within each joint limits. @return a dictionary with the joint names as keys and current joint values """ current = self._move_group_commander._g.get_current_state_bounded() names = self._move_group_commander._g.get_active_joints() output = {n: current[n] for n in names if n in current} return output def get_robot_state_bounded(self): return self._move_group_commander._g.get_current_state_bounded() def move_to_named_target(self, name, wait=True): """ Set target of the robot's links and moves to it @param name - name of the target pose defined in SRDF @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self._move_group_commander.go(wait=wait) def plan_to_named_target(self, name): """ Set target of the robot's links and plans This is a blocking method. @param name - name of the target pose defined in SRDF """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self.__plan = self._move_group_commander.plan() def __get_warehouse_names(self): try: list_srv = rospy.ServiceProxy("list_robot_states", ListStates) return list_srv("", self._robot_name).states except rospy.ServiceException as exc: rospy.logwarn("Couldn't access warehouse: " + str(exc)) return list() def _reset_plan(self): self.__plan = None def _set_plan(self, plan): self.__plan = plan def __get_srdf_names(self): return self._move_group_commander._g.get_named_targets() def get_named_targets(self): """ Get the complete list of named targets, from SRDF as well as warehouse poses if available. @return list of strings containing names of targets. """ return self._srdf_names + self._warehouse_names def get_joints_position(self): """ Returns joints position @return - dictionary with joints positions """ with self._joint_states_lock: return self._joints_position def get_joints_velocity(self): """ Returns joints velocities @return - dictionary with joints velocities """ with self._joint_states_lock: return self._joints_velocity def _get_joints_effort(self): """ Returns joints effort @return - dictionary with joints efforts """ with self._joint_states_lock: return self._joints_effort def get_joints_state(self): """ Returns joints state @return - JointState message """ with self._joint_states_lock: return self._joints_state def run_joint_trajectory(self, joint_trajectory): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. """ plan = RobotTrajectory() plan.joint_trajectory = joint_trajectory self._move_group_commander.execute(plan) def make_named_trajectory(self, trajectory): """ Makes joint value trajectory from specified by named poses (either from SRDF or from warehouse) @param trajectory - list of waypoints, each waypoint is a dict with the following elements (n.b either name or joint_angles is required) - name -> the name of the way point - joint_angles -> a dict of joint names and angles - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent. """ current = self.get_current_state_bounded() joint_trajectory = JointTrajectory() joint_names = current.keys() joint_trajectory.joint_names = joint_names start = JointTrajectoryPoint() start.positions = current.values() start.time_from_start = rospy.Duration.from_sec(0.001) joint_trajectory.points.append(start) time_from_start = 0.0 for wp in trajectory: joint_positions = None if 'name' in wp.keys(): joint_positions = self.get_named_target_joint_values( wp['name']) elif 'joint_angles' in wp.keys(): joint_positions = copy.deepcopy(wp['joint_angles']) if 'degrees' in wp.keys() and wp['degrees']: for joint, angle in joint_positions.iteritems(): joint_positions[joint] = radians(angle) if joint_positions is None: rospy.logerr( "Invalid waypoint. Must contain valid name for named target or dict of joint angles." ) return None new_positions = {} for n in joint_names: new_positions[n] = joint_positions[ n] if n in joint_positions else current[n] trajectory_point = JointTrajectoryPoint() trajectory_point.positions = [ new_positions[n] for n in joint_names ] current = new_positions time_from_start += wp['interpolate_time'] trajectory_point.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(trajectory_point) if 'pause_time' in wp and wp['pause_time'] > 0: extra = JointTrajectoryPoint() extra.positions = trajectory_point.positions time_from_start += wp['pause_time'] extra.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(extra) return joint_trajectory def send_stop_trajectory_unsafe(self): """ Sends a trajectory of all active joints at their current position. This stops the robot. """ current = self.get_current_state_bounded() trajectory_point = JointTrajectoryPoint() trajectory_point.positions = current.values() trajectory_point.time_from_start = rospy.Duration.from_sec(0.1) trajectory = JointTrajectory() trajectory.points.append(trajectory_point) trajectory.joint_names = current.keys() self.run_joint_trajectory_unsafe(trajectory) def run_named_trajectory_unsafe(self, trajectory, wait=False): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory directly via contoller. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory_unsafe(joint_trajectory, wait) def run_named_trajectory(self, trajectory): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory via moveit. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory(joint_trajectory) def move_to_position_target(self, xyz, end_effector_link="", wait=True): """ Specify a target position for the end-effector and moves to it @param xyz - new position of end-effector @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_position_target(self, xyz, end_effector_link=""): """ Specify a target position for the end-effector and plans. This is a blocking method. @param xyz - new position of end-effector @param end_effector_link - name of the end effector link """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self.__plan = self._move_group_commander.plan() def move_to_pose_target(self, pose, end_effector_link="", wait=True): """ Specify a target pose for the end-effector and moves to it @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_pose_target(pose, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_pose_target(self, pose, end_effector_link="", alternative_method=False): """ Specify a target pose for the end-effector and plans. This is a blocking method. @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param alternative_method - use set_joint_value_target instead of set_pose_target """ self._move_group_commander.set_start_state_to_current_state() if alternative_method: self._move_group_commander.set_joint_value_target( pose, end_effector_link) else: self._move_group_commander.set_pose_target(pose, end_effector_link) self.__plan = self._move_group_commander.plan() return self.__plan def _joint_states_callback(self, joint_state): """ The callback function for the topic joint_states. It will store the received joint position, velocity and efforts information into dictionaries @param joint_state - the message containing the joints data. """ with self._joint_states_lock: self._joints_state = joint_state self._joints_position = { n: p for n, p in zip(joint_state.name, joint_state.position) } self._joints_velocity = { n: v for n, v in zip(joint_state.name, joint_state.velocity) } self._joints_effort = { n: v for n, v in zip(joint_state.name, joint_state.effort) } def _set_up_action_client(self, controller_list): """ Sets up an action client to communicate with the trajectory controller """ self._action_running = {} for controller_name in controller_list.keys(): self._action_running[controller_name] = False service_name = controller_name + "/follow_joint_trajectory" self._clients[controller_name] = SimpleActionClient( service_name, FollowJointTrajectoryAction) if self._clients[controller_name].wait_for_server( timeout=rospy.Duration(4)) is False: err_msg = 'Failed to connect to action server ({}) in 4 sec'.format( service_name) rospy.logwarn(err_msg) def move_to_joint_value_target_unsafe(self, joint_states, time=0.002, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param time - time in s (counting from now) for the robot to reach the target (it needs to be greater than 0.0 for it not to be rejected by the trajectory controller) @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ # self._update_default_trajectory() # self._set_targets_to_default_trajectory(joint_states) goals = {} joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [] point = JointTrajectoryPoint() point.positions = [] for x in joint_states_cpy.keys(): if x in controller_joints: goal.trajectory.joint_names.append(x) point.positions.append(joint_states_cpy[x]) point.time_from_start = rospy.Duration.from_sec(time) goal.trajectory.points = [point] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def action_is_running(self, controller=None): if controller is not None: return self._action_running[controller] for controller_running in self._action_running.values(): if controller_running: return True return False def _action_done_cb(self, controller, terminal_state, result): self._action_running[controller] = False def _call_action(self, goals): for client in self._clients: self._action_running[client] = True self._clients[client].send_goal( goals[client], lambda terminal_state, result: self._action_done_cb( client, terminal_state, result)) def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. @param wait - should method wait for movement end or not """ goals = {} for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory = copy.deepcopy(joint_trajectory) indices_of_joints_in_this_controller = [] for i, joint in enumerate(joint_trajectory.joint_names): if joint in controller_joints: indices_of_joints_in_this_controller.append(i) goal.trajectory.joint_names = [ joint_trajectory.joint_names[i] for i in indices_of_joints_in_this_controller ] for point in goal.trajectory.points: if point.positions: point.positions = [ point.positions[i] for i in indices_of_joints_in_this_controller ] if point.velocities: point.velocities = [ point.velocities[i] for i in indices_of_joints_in_this_controller ] if point.effort: point.effort = [ point.effort[i] for i in indices_of_joints_in_this_controller ] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def plan_to_waypoints_target(self, waypoints, reference_frame=None, eef_step=0.005, jump_threshold=0.0): """ Specify a set of waypoints for the end-effector and plans. This is a blocking method. @param reference_frame - the reference frame in which the waypoints are given @param waypoints - an array of poses of end-effector @param eef_step - configurations are computed for every eef_step meters @param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path """ old_frame = self._move_group_commander.get_pose_reference_frame() if reference_frame is not None: self.set_pose_reference_frame(reference_frame) (self.__plan, fraction) = self._move_group_commander.compute_cartesian_path( waypoints, eef_step, jump_threshold) self.set_pose_reference_frame(old_frame) def set_teach_mode(self, teach): """ Activates/deactivates the teach mode for the robot. Activation: stops the the trajectory controllers for the robot, and sets it to teach mode. Deactivation: stops the teach mode and starts trajectory controllers for the robot. Currently this method blocks for a few seconds when called on a hand, while the hand parameters are reloaded. @param teach - bool to activate or deactivate teach mode """ if teach: mode = RobotTeachModeRequest.TEACH_MODE else: mode = RobotTeachModeRequest.TRAJECTORY_MODE self.change_teach_mode(mode, self._name) def move_to_trajectory_start(self, trajectory, wait=True): """ Make and execute a plan from the current state to the first state in an pre-existing trajectory @param trajectory - moveit_msgs/JointTrajectory @param wait - Bool to specify if movement should block untill finished. """ if len(trajectory.points) <= 0: rospy.logerr("Trajectory has no points in it, can't reverse...") return None first_point = trajectory.points[0] end_state = dict(zip(trajectory.joint_names, first_point.positions)) self.move_to_joint_value_target(end_state, wait=wait) @staticmethod def change_teach_mode(mode, robot): teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode) req = RobotTeachModeRequest() req.teach_mode = mode req.robot = robot try: resp = teach_mode_client(req) if resp.result == RobotTeachModeResponse.ERROR: rospy.logerr("Failed to change robot %s to mode %d", robot, mode) else: rospy.loginfo("Changed robot %s to mode %d Result = %d", robot, mode, resp.result) except rospy.ServiceException: rospy.logerr("Failed to call service teach_mode") def get_ik(self, target_pose, avoid_collisions=False, joint_states=None): """ Computes the inverse kinematics for a given pose. It returns a JointState @param target_pose - A given pose of type PoseStamped @param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false """ service_request = PositionIKRequest() service_request.group_name = self._name service_request.ik_link_name = self._move_group_commander.get_end_effector_link( ) service_request.pose_stamped = target_pose service_request.timeout.secs = 0.5 service_request.avoid_collisions = avoid_collisions if joint_states is None: service_request.robot_state.joint_state = self.get_joints_state() else: service_request.robot_state.joint_state = joint_states try: resp = self._compute_ik(ik_request=service_request) # Check if error_code.val is SUCCESS=1 if resp.error_code.val != 1: if resp.error_code.val == -10: rospy.logerr("Unreachable point: Start state in collision") elif resp.error_code.val == -12: rospy.logerr("Unreachable point: Goal state in collision") elif resp.error_code.val == -31: rospy.logerr("Unreachable point: No IK solution") else: rospy.logerr("Unreachable point (error: %s)" % resp.error_code) return else: return resp.solution.joint_state except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface("base_link") # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the UBR-1 find_objects action server rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Begin the main perception and pick-and-place loop while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the UBR-1 grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(5.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) # Remove all previous objects from the planning scene for name in scene.getKnownCollisionObjects(): scene.removeCollisionObject(name, False) for name in scene.getKnownAttachedObjects(): scene.removeAttachedObject(name, False) scene.waitForSync() # Clear the virtual object colors scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_size = None the_object = None the_object_dist = 1.0 count = -1 # Cycle through all detected objects and keep the nearest one for obj in find_result.objects: count += 1 scene.addSolidPrimitive("object%d"%count, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Choose the object nearest to the robot dx = obj.object.primitive_poses[0].position.x - args.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist: the_object_dist = d the_object = count # Get the size of the target target_size = obj.object.primitives[0].dimensions # Set the target pose target_pose.pose = obj.object.primitive_poses[0] # We want the gripper to be horizontal target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d"%the_object # Insert the support surface into the planning scene for obj in find_result.support_surfaces: # Extend surface to floor height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 2.0, # make table wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # Add to scene scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) # Get the table dimensions table_size = obj.primitives[0].dimensions # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync scene.waitForSync() # Set colors of the table and the object we are grabbing scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey scene.sendColors() # Skip pick-and-place if we are just detecting objects if args.objects: if args.once: exit(0) else: continue # Get the support surface ID support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object right_arm.set_support_surface_name(support_surface) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = target_pose.pose.position.x place_pose.pose.position.y = 0.03 place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] / 2.0 grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") continue # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Set the start state to the current state #right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(2) # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(2) # Give the servos a rest arbotix_relax_all_servos() rospy.sleep(2) if args.once: # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) cartesian = rospy.get_param('~cartesian', True) # Connect to the arm move group arm = MoveGroupCommander('robot') gripper = MoveGroupCommander("gripper") # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame('base_link') # Allow some leeway in position(meters) and orientation (radians) # arm.set_goal_position_tolerance(0.01) # arm.set_goal_orientation_tolerance(0.1) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.1) #(0.523599) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() gripper.set_named_target("GripperOpen") gripper.go(wait=True) rospy.sleep(3) #5 gripper.set_named_target("GripperClose") gripper.go(wait=True) rospy.sleep(1) # # Start in the "cartesian_z" (demo) configuration stored in the SRDF file arm.set_named_target('cartesian_z') # # Plan and execute a trajectory to the goal configuration arm.go(wait=True) rospy.sleep(5) # Execute the position again in order to start from there - !!!!workaround # arm.set_named_target('cartesian_z') # arm.go(wait=True) # Get the current pose so we can add it as a waypoint start_pose = arm.get_current_pose(end_effector_link).pose # Initialize the waypoints list waypoints = [] # Set the first waypoint to be the starting pose if cartesian: # Append the pose to the waypoints list waypoints.append(start_pose) wpose = deepcopy(start_pose) # Set the next waypoint back 0.2 meters and right 0.2 meters wpose.position.x -= 0.0 #05#0.01 wpose.position.y -= 0.0 wpose.position.z -= 0.25 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # Set the next waypoint to the right 0.15 meters wpose.position.x += 0.0 #0.05#0.01 wpose.position.y += 0.0 wpose.position.z += 0.38 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(wpose)) else: arm.set_pose_target(wpose) arm.go() rospy.sleep(1) # Set the next waypoint to the right 0.15 meters wpose.position.x += 0.0 #0.05#0.01 wpose.position.y += 0.0 wpose.position.z -= 0.38 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(start_pose)) else: arm.set_pose_target(start_pose) arm.go() rospy.sleep(1) wpose.position.x += 0.0 wpose.position.y += 0.0 wpose.position.z += 0.25 if cartesian: # Append the pose to the waypoints list waypoints.append(deepcopy(start_pose)) else: arm.set_pose_target(start_pose) arm.go() rospy.sleep(1) if cartesian: fraction = 0.0 maxtries = 100 #100 attempts = 0 # Set the internal state to the current state arm.set_start_state_to_current_state() # Plan the Cartesian path connecting the waypoints while fraction < 1.0 and attempts < maxtries: (plan, fraction) = arm.compute_cartesian_path( waypoints, # waypoint poses 0.01, # eef_step 0.0, # jump_threshold True) # avoid_collisions # Increment the number of attempts attempts += 1 # Print out a progress message if attempts % 10 == 0: rospy.loginfo("Still trying after " + str(attempts) + " attempts...") # If we have a complete plan, execute the trajectory if fraction == 1.0: rospy.loginfo("Path computed successfully. Moving the arm.") arm.execute(plan, wait=True) rospy.loginfo("Path execution complete.") else: rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") # Move normally back to the 'transport_position' position # arm.set_named_target('cartesian_z') # arm.go(wait=True) rospy.sleep(22) arm.set_named_target('transport_position') arm.go(wait=True) rospy.sleep(27) gripper.set_named_target("GripperOpen") gripper.go(wait=True) rospy.sleep(5) gripper.set_named_target("GripperClose") gripper.go(wait=True) rospy.sleep(5) arm.set_named_target('transport_position') arm.go(wait=True) rospy.sleep(27) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 10 seconds per planning attempt right_arm.set_planning_time(10) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 10 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.65 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.55 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.55 box1_pose.pose.position.y = -0.1 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.54 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.60 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table red and the boxes orange self.setColor(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.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object right_arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.50 place_pose.pose.position.y = -0.25 place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it grasp_pose.pose.position.y -= target_size[1] / 2.0 # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = right_arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 #_------------------------now we move to the other table__________------------------------------------------- #_------------------------now we move to the other table__________------------------------------------------- # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('right_start') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' tool_id = 'tool' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) scene.remove_world_object(tool_id) # Remove any attached objects from a previous session scene.remove_attached_object(GRIPPER_FRAME, target_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "grasp" pose stored in the SRDF file arm.set_named_target('left_arm_up') arm.go() # Open the gripper to the neutral position gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # Set the height of the table off the ground table_ground = 0.04 # Set the dimensions of the scene objects [l, w, h] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Set the target size [l, w, h] target_size = [0.02, 0.01, 0.12] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.25 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.21 box1_pose.pose.position.y = -0.1 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.19 box2_pose.pose.position.y = 0.13 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 #scene.add_box(box2_id, box2_pose, box2_size) # Set the target pose in between the boxes and on the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.24 target_pose.pose.position.y = 0.275 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # Add the target object to the scene scene.add_box(target_id, target_pose, target_size) # Make the table blue and the boxes orange self.setColor(table_id, 0, 0, 0.8, 1.0) self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box2_id, 0.8, 0.4, 0, 1.0) # Make the target yellow self.setColor(target_id, 0.9, 0.9, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the support surface name to the table object arm.set_support_surface_name(table_id) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.18 place_pose.pose.position.y = 0 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose by half the width of the target to center it #grasp_pose.pose.position.y -= target_size[1] / 2.0 grasp_pose.pose.position.x = 0.12792118579 + .1 grasp_pose.pose.position.y = 0.285290879999 + 0.05 grasp_pose.pose.position.z = 0.120301181892 #grasp_pose.pose.orientation = # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id, table_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) break # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = arm.pick(target_id, grasps) rospy.sleep(0.2) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('left_arm_rest') arm.go() # Open the gripper to the open position gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
while rospy.get_time() == 0.0: pass ### Create a handle for the Planning Scene Interface psi = PlanningSceneInterface() rospy.sleep(1.0) ### Create a handle for the Move Group Commander mgc = MoveGroupCommander("arm") rospy.sleep(1.0) ### Add virtual obstacle pose = gen_pose(pos=[-0.2, -0.1, 1.2]) psi.add_box("box", pose, size=(0.15, 0.15, 0.6)) rospy.sleep(1.0) ### Move to stored joint position mgc.set_named_target("left") mgc.go() ### Move to Cartesian position goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707]) mgc.go(goal_pose.pose) ### Move Cartesian linear goal_pose.pose.position.z -= 0.1 (traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True) mgc.execute(traj) print "Done"
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_pick_and_place_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # 创建一个发布抓取姿态的发布者 self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander(GROUP_NAME_ARM) # 初始化需要使用move group控制的机械臂中的gripper group gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 arm.set_pose_reference_frame(REFERENCE_FRAME) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置pick和place阶段的最大尝试次数 max_pick_attempts = 5 max_place_attempts = 5 rospy.sleep(2) # 设置场景物体的名称 table_id = 'table' box1_id = 'box1' box2_id = 'box2' target_id = 'target' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(target_id) # 移除场景中之前与机器臂绑定的物体 scene.remove_attached_object(GRIPPER_FRAME, target_id) rospy.sleep(1) # 控制机械臂先回到初始化位置 arm.set_named_target('home') arm.go() # 控制夹爪张开 gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # 设置桌面的高度 table_ground = 0.2 # 设置table、box1和box2的三维尺寸[长, 宽, 高] table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.31 box1_pose.pose.position.y = -0.1 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.29 box2_pose.pose.position.y = -0.4 box2_pose.pose.position.z = table_ground + table_size[ 2] + box2_size[2] / 2.0 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.4, 0, 1.0) # 设置目标物体的尺寸 target_size = [0.04, 0.04, 0.05] # 设置目标物体的位置,位于桌面之上两个盒子之间 target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.32 target_pose.pose.position.y = 0.0 target_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 target_pose.pose.orientation.w = 1.0 # 将抓取的目标物体加入场景中 scene.add_box(target_id, target_pose, target_size) # 将目标物体设置为黄色 self.setColor(target_id, 0.9, 0.9, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # 设置支持的外观 arm.set_support_surface_name(table_id) # 设置一个place阶段需要放置物体的目标位置 place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = 0.32 place_pose.pose.position.y = -0.2 place_pose.pose.position.z = table_ground + table_size[ 2] + target_size[2] / 2.0 place_pose.pose.orientation.w = 1.0 # 将目标位置设置为机器人的抓取目标位置 grasp_pose = target_pose # 生成抓取姿态 grasps = self.make_grasps(grasp_pose, [target_id]) # 将抓取姿态发布,可以在rviz中显示 for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # 追踪抓取成功与否,以及抓取的尝试次数 result = None n_attempts = 0 # 重复尝试抓取,直道成功或者超多最大尝试次数 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) result = arm.pick(target_id, grasps) rospy.sleep(0.2) # 如果pick成功,则进入place阶段 if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # 生成放置姿态 places = self.make_places(place_pose) # 重复尝试放置,直道成功或者超多最大尝试次数 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") # 控制机械臂回到初始化位置 arm.set_named_target('home') arm.go() # 控制夹爪回到张开的状态 gripper.set_joint_value_target(GRIPPER_OPEN) gripper.go() rospy.sleep(1) # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_obstacles_demo') # Construct the initial scene object scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a dictionary to hold object colors self.colors = dict() # Pause for the scene to get ready rospy.sleep(1) # Initialize the move group for the right arm arm = MoveGroupCommander(GROUP_NAME_ARM) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame accordingly arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt arm.set_planning_time(5) # Give each of the scene objects a unique name table_id = 'table' box1_id = 'box1' box2_id = 'box2' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() rospy.sleep(2) # Set the height of the table off the ground table_ground = 0.65 # Set the length, width and height of the table and boxes table_size = [0.2, 0.7, 0.01] box1_size = [0.1, 0.05, 0.05] box2_size = [0.05, 0.05, 0.15] # Add a table top and two boxes to the scene table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0.35 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME box1_pose.pose.position.x = 0.3 box1_pose.pose.position.y = 0 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.3 box2_pose.pose.position.y = 0.25 box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # Make the table red and the boxes orange 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.4, 0, 1.0) # Send the colors to the planning scene self.sendColors() # Set the target pose in between the boxes and above the table target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.22 target_pose.pose.position.y = 0.14 target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the target pose for the arm arm.set_pose_target(target_pose, end_effector_link) # Move the arm to the target pose (if possible) arm.go() # Pause for a moment... rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file arm.set_named_target('l_arm_init') arm.go() # Exit MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
class ArmTracker: def __init__(self): rospy.init_node('arm_tracker') rospy.on_shutdown(self.shutdown) # Maximum distance of the target before the arm will lower self.max_target_dist = 1.2 # Arm length to center of gripper frame self.arm_length = 0.4 # Distance between the last target and the new target before we move the arm self.last_target_threshold = 0.01 # Distance between target and end-effector before we move the arm self.target_ee_threshold = 0.025 # Initialize the move group for the left arm self.left_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the left gripper left_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Set the reference frame for pose targets self.reference_frame = REFERENCE_FRAME # Keep track of the last target pose self.last_target_pose = PoseStamped() # Set the left arm reference frame accordingly self.left_arm.set_pose_reference_frame(self.reference_frame) # Allow replanning to increase the chances of a solution self.left_arm.allow_replanning(False) # Set a position tolerance in meters self.left_arm.set_goal_position_tolerance(0.05) # Set an orientation tolerance in radians self.left_arm.set_goal_orientation_tolerance(0.2) # What is the end effector link? self.ee_link = self.left_arm.get_end_effector_link() # Create the transform listener self.listener = tf.TransformListener() # Queue up some tf data... rospy.sleep(3) # Set the gripper target to closed position using a joint value target left_gripper.set_joint_value_target(GRIPPER_CLOSED) # Plan and execute the gripper motion left_gripper.go() rospy.sleep(1) # Subscribe to the target topic rospy.wait_for_message('/target_pose', PoseStamped) # Use queue_size=1 so we don't pile up outdated target messages self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1) rospy.loginfo("Ready for action!") while not rospy.is_shutdown(): try: target = self.target except: rospy.sleep(0.5) continue # Timestamp the target with the current time target.header.stamp = rospy.Time() # Get the target pose in the left_arm shoulder lift frame #target_arm = self.listener.transformPose('left_arm_shoulder_pan_link', target) target_arm = self.listener.transformPose('left_rotate', target) # Convert the position values to a Python list p0 = [ target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z ] # Compute the distance between the target and the shoulder link dist_target_shoulder = euclidean(p0, [0, 0, 0]) # If the target is too far away, then lower the arm if dist_target_shoulder > self.max_target_dist: rospy.loginfo("Target is too far away") self.left_arm.set_named_target('l_start') self.left_arm.go() rospy.sleep(1) continue # Transform the pose to the base reference frame target_base = self.listener.transformPose(self.reference_frame, target) # Compute the distance between the current target and the last target p1 = [ target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z ] p2 = [ self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z ] dist_last_target = euclidean(p1, p2) # Move the arm only if we are far enough away from the previous target if dist_last_target < self.last_target_threshold: rospy.loginfo("Still close to last target") rospy.sleep(0.5) continue # Get the pose of the end effector in the base reference frame ee_pose = self.left_arm.get_current_pose(self.ee_link) # Convert the position values to a Python list p3 = [ ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z ] # Compute the distance between the target and the end-effector dist_target = euclidean(p1, p3) # Only move the arm if we are far enough away from the target if dist_target < self.target_ee_threshold: rospy.loginfo("Already close enough to target") rospy.sleep(1) continue # We want the gripper somewhere on the line connecting the shoulder and the target. # Using a parametric form of the line, the parameter ranges from 0 to the # minimum of the arm length and the distance to the target. t_max = min(self.arm_length, dist_target_shoulder) # Bring it back 10% so we don't collide with the target t = 0.9 * t_max # Now compute the target positions from the parameter try: target_arm.pose.position.x *= (t / dist_target_shoulder) target_arm.pose.position.y *= (t / dist_target_shoulder) target_arm.pose.position.z *= (t / dist_target_shoulder) except: rospy.sleep(1) rospy.loginfo("Exception!") continue # Transform to the base_footprint frame target_ee = self.listener.transformPose(self.reference_frame, target_arm) # Set the target gripper orientation to be horizontal target_ee.pose.orientation.x = 0 target_ee.pose.orientation.y = 0 target_ee.pose.orientation.z = 0 target_ee.pose.orientation.w = 1 # Update the current start state self.left_arm.set_start_state_to_current_state() # Set the target pose for the end-effector self.left_arm.set_pose_target(target_ee, self.ee_link) # Plan and execute the trajectory success = self.left_arm.go() if success: # Store the current target as the last target self.last_target_pose = target # Pause a bit between motions to keep from locking up rospy.sleep(0.5) def update_target_pose(self, target): self.target = target def relax_all_servos(self): command = 'rosrun rbx2_dynamixels arbotix_relax_all_servos.py' args = shlex.split(command) subprocess.Popen(args) def shutdown(self): # Stop any further target messages from being processed self.target_subscriber.unregister() # Stop any current arm movement self.left_arm.stop() # Move to the r_start position self.left_arm.set_named_target('r_start') self.left_arm.go() # Relax the servos self.relax_all_servos() os._exit(0)
def __init__(self, script_path): rospy.init_node('show_time') rospy.on_shutdown(self.cleanup) # Initialize a number of parameters and variables for nav locations # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Initialize the move group for the arms right_arm = MoveGroupCommander('right_arm') left_arm = MoveGroupCommander('left_arm') right_gripper = MoveGroupCommander('right_gripper') left_gripper = MoveGroupCommander('left_gripper') # Set the default TTS voice to use self.voice = rospy.get_param("~voice", "voice_don_diphone") self.robot = rospy.get_param("~robot", "robbie") self.start =(Pose(Point(1.7, 0, 0.0), Quaternion(0.0, 0.0, 1.000, 0.018))) self.left =(Pose(Point(1.960, -1.854, 0.0), Quaternion(0.0, 0.0, 0.916, 0.401))) self.right =(Pose(Point(2.123, 1.592, 0.0), Quaternion(0.0, 0.0, 0.877, -0.480))) self.auto_dock =(Pose(Point(0.5, 0, 0.0), Quaternion(0.0, 0.0, 0.002, 1.000))) self.client = SimpleActionClient("move_base", MoveBaseAction) self.client.wait_for_server() # Create the sound client object self.soundhandle = SoundClient() # Wait a moment to let the client connect to the # sound_play server rospy.sleep(2) # Make sure any lingering sound_play processes are stopped. self.soundhandle.stopAll() # Set the wave file path if used self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds") #define afternoon and morning self.noon = strftime("%p:", localtime()) if self.noon == "AM:": self.noon1 = "Goood Morning " else: self.noon1 ="Good After Noon " self.local = strftime("%H:%M:", localtime()) #local time self.local = strftime("%H:%M:", localtime()) self.soundhandle.say(self.noon1 + self.robot +" is on line" + " the time is " + self.local, self.voice) right_arm.set_named_target('r_travel') right_arm.go() left_arm.set_named_target('l_travel') left_arm.go() rospy.sleep(5) self.move_to(self.start) self.soundhandle.say("Welcome to SHOW time. This is where I get to demonstrate my capabilities" , self.voice) rospy.sleep(6) self.soundhandle.say("I can move to my left", self.voice) rospy.sleep(2) self.move_to(self.left) self.soundhandle.say("now back to the start position", self.voice) rospy.sleep(2) self.move_to(self.start) self.soundhandle.say("I can move to my right", self.voice) rospy.sleep(2) self.move_to(self.right) self.soundhandle.say("And again back to the start position", self.voice) rospy.sleep(2) self.move_to(self.start) self.soundhandle.say("I once caught a fish this big", self.voice) right_arm.set_named_target('r_fish') right_arm.go() left_arm.set_named_target('l_fish') left_arm.go() #rospy.sleep(2) self.soundhandle.say("Thank you for your time ", self.voice) right_arm.set_named_target('right_start') right_arm.go() left_arm.set_named_target('left_start') left_arm.go() #rospy.sleep(2) #rospy.sleep(2) self.move_to(self.auto_dock) # add auto dock sequence here # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)