def set_controller(self, mode, speed_scalar, ft_threshold): req = SetControllerModeRequest() req.mode.mode = mode req.speed_scalar = speed_scalar req.force_torque_stop_threshold = ft_threshold res = self.set_controller_mode(req) if (not res.success): raise Exception("Could not set controller mode")
## MoveIt! Initialization robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("move_group") group.set_goal_position_tolerance(0.04) group.allow_replanning(True) group.set_planner_id("RRTConnectkConfigDefault") #RRTConnectkConfigDefault/SBLkConfigDefault/KPIECEkConfigDefault/BKPIECEkConfigDefault/LBKPIECEkConfigDefault/ group.set_num_planning_attempts(5) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) #ROS service calls to check system variables set_controller_mode=rospy.ServiceProxy('set_controller_mode', SetControllerMode) set_digital_io=rospy.ServiceProxy('rapid/set_digital_io', RapidSetDigitalIO) req=SetControllerModeRequest() req.mode.mode=4 req.speed_scalar=0.7 req.force_torque_stop_threshold=ft_threshold res=set_controller_mode(req) if (not res.success): raise Exception("Could not set controller mode") rospy.sleep(2) print "============ Printing robot Pose" print group.get_current_pose().pose tic = timeit.default_timer()
class VisionMoveIt: def __init__(self): self.ft_threshold=[250,250,250,250,250,250] self.Force_Measurement=0 self.pose_target=0 #could also be inserted into the starting section of control code for better visibility set_controller_mode=rospy.ServiceProxy('set_controller_mode', SetControllerMode) set_digital_io=rospy.ServiceProxy('rapid/set_digital_io', RapidSetDigitalIO) def camera_init(self): self.P,self.Q=CameraService() def moveit_init(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('collision_checker','move_group_python_interface_tutorial', anonymous=True) ## MoveIt! Initialization robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander("move_group") group.set_goal_position_tolerance(0.04) group.allow_replanning(True) group.set_planner_id("RRTConnectkConfigDefault") #RRTConnectkConfigDefault/SBLkConfigDefault/KPIECEkConfigDefault/BKPIECEkConfigDefault/LBKPIECEkConfigDefault/ group.set_num_planning_attempts(5) display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) self.group=group #ROS service calls to check system variables def generate_plans(self): print "============ Printing robot Pose" print self.group.get_current_pose().pose Q=self.Q P=self.P tic = timeit.default_timer() dt = 0 while dt< 3: toc = timeit.default_timer() dt = toc - tic print 'Start' print "============ Printing robot Pose" print group.get_current_pose() #print robot.get_current_state().joint_state.position print "============ Generating plan 1" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = Q[1] pose_target.orientation.y = Q[2]#0.707 pose_target.orientation.z = Q[3]#0.707 pose_target.orientation.w = Q[0]#qoa[3] #0#0 pose_target.position.x = P[0][0] pose_target.position.y = P[0][1]#-2.02630600362 pose_target.position.z = P[0][2] + 0.3 self.pose_target=pose_target #self.group.set_pose_target(pose_target) print "============ Printing robot Pose" print group.get_current_pose() print "============ Generating plan 2" pose_target2 = copy.deepcopy(pose_target) pose_target2.position.z -= 0.45 self.pose_target2=pose_target2 req=SetControllerModeRequest() req.mode.mode=4 req.speed_scalar=1 req.force_torque_stop_threshold=ft_threshold res=set_controller_mode(req) if (not res.success): raise Exception("Could not set controller mode") #rospy.sleep(2) if (1): print "============ Printing robot Pose" print group.get_current_pose() #print robot.get_current_state().joint_state.position print "============ Generating plan 1" pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = Q[1] pose_target.orientation.y = Q[2]#0.707 pose_target.orientation.z = Q[3]#0.707 pose_target.orientation.w = Q[0]#qoa[3] #0#0 pose_target.position.x = P[0][0] pose_target.position.y = P[0][1]#-2.02630600362 pose_target.position.z = P[0][2] + 0.3 group.set_pose_target(pose_target) ''' pose_target = geometry_msgs.msg.Pose() pose_target.position.x = -0.02892 pose_target.position.y = -1.79035 pose_target.position.z = 0.914083 pose_target.orientation.x = 0.426720076618065 pose_target.orientation.y = 0.5339800423981502 pose_target.orientation.z = -0.4531605121430878 pose_target.orientation.w = 0.5722069911891644 group.set_pose_target(pose_target) ''' print 'Target:',pose_target ## Now, we call the planner to compute the plan ## and visualize it if successful ## Note that we are just planning, not asking move_group ## to actually move the robot plan1 = group.plan() print plan1 cnt = 0 while( (not plan1.joint_trajectory.points) and (cnt<3)): print "============ Generating plan 1" plan1 = group.plan() cnt = cnt+1 raw_input("Press Enter to continue") print "============ Executing plan1" group.execute(plan1) print 'Execution Finished.' ########## Vertical Path ############ req=SetControllerModeRequest() req.mode.mode=4 req.speed_scalar=0.4 req.force_torque_stop_threshold=ft_threshold res=set_controller_mode(req) if (not res.success): raise Exception("Could not set controller mode") """pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = Q[1] pose_target.orientation.y = Q[2]#0.707 pose_target.orientation.z = Q[3]#0.707 pose_target.orientation.w = Q[0]#qoa[3] #0#0 pose_target.position.x = P[0][0] pose_target.position.y = P[0][1]#-2.02630600362 pose_target.position.z = P[0][2] + 0.2""" group.set_pose_target(pose_target2) print 'Target:',pose_target2 ## Now, we call the planner to compute the plan ## and visualize it if successful ## Note that we are just planning, not asking move_group ## to actually move the robot plan2 = group.plan() cnt = 0 while( (not plan2.joint_trajectory.points) and (cnt<3)): print "============ Generating plan 2" plan2 = group.plan() cnt = cnt+1 time.sleep(1) print "============ Executing plan2" group.execute(plan2) print 'Execution Finished.' req=SetControllerModeRequest() req.mode.mode=4 req.speed_scalar=0.7 req.force_torque_stop_threshold=[] res=set_controller_mode(req) if (not res.success): raise Exception("Could not set controller mode") print "============ Lift panel!" req=RapidSetDigitalIORequest() req.signal="Vacuum_enable" req.lvalue=1 set_digital_io(req) pose_target3 = copy.deepcopy(pose_target) pose_target3.position.z += 0.25 group.set_pose_target(pose_target3) print 'Target:',pose_target3 ## Now, we call the planner to compute the plan ## and visualize it if successful ## Note that we are just planning, not asking move_group ## to actually move the robot plan3 = group.plan() cnt = 0 while( (not plan3.joint_trajectory.points) and (cnt<3)): print "============ Generating plan 3" plan3 = group.plan() cnt = cnt+1 time.sleep(1) print "============ Executing plan3" group.execute(plan3) print 'Execution Finished.' '''
rospy.sleep(2) print "============ Printing robot Pose" print group.get_current_pose().pose tic = timeit.default_timer() dt = 0 while dt < 3: toc = timeit.default_timer() dt = toc - tic print 'Start' set_controller_mode = rospy.ServiceProxy('set_controller_mode', SetControllerMode) req = SetControllerModeRequest() req.mode.mode = 0 req.speed_scalar = 0.5 req.force_torque_stop_threshold = [] req = SetControllerModeRequest() req.mode.mode = 4 req.speed_scalar = 0.5 req.force_torque_stop_threshold = [] res = set_controller_mode(req) if (not res.success): raise Exception("Could not set controller mode") print "============ Printing robot Pose" print group.get_current_pose() #print robot.get_current_state().joint_state.position