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")
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() dt = 0 while dt< 3:
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 = 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) 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 "============ 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 print "============ Generating plan 1" pose_target = geometry_msgs.msg.Pose()