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