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()