def __init__(self):
self._start_pose = {
'position':
Point(x=0.83580435659716,
y=0.14724066320869522,
z=0.4548728070777701),
'orientation':
Quaternion(x=0.7366681513777151,
y=-0.6758459333695754,
z=0.023326822897651703,
w=0.002858046019378824)
}
self._pixel_meter_ratio = 0.001
self._threshold = 4
self.harmonic_repeat = float(2)
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
self.compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self.right_gripper = robot_gripper.Gripper('right_gripper')
self.limb = Limb()
self._planning_scene_publisher = rospy.Publisher('/collision_object',
CollisionObject,
queue_size=10)
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
right_gripper = robot_gripper.Gripper('right_gripper')
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
print('Calibrating gripper...')
right_gripper.calibrate()
rospy.sleep(2.0)
move_to(pos_x=0.676, pos_y=0.120, pos_z=0.146, \
orien_x=0, orien_y=1, orien_z=0, orien_w=0, ik=compute_ik)
right_gripper.close()
rospy.sleep(1.0)
move_to(pos_x=0.676, pos_y=0.120, pos_z=-0.146, \
orien_x=0, orien_y=1, orien_z=0, orien_w=0, ik=compute_ik)
right_gripper.open()
rospy.sleep(1.0)
def gripper_close(self):
"""S
Close the gripper
"""
print('Closing...')
robot_gripper.Gripper('right')
right_gripper.close()
def gripper_open(self):
"""
Open the gripper
"""
print('Opening...')
right_gripper = robot_gripper.Gripper('right')
right_gripper.calibrate()
rospy.sleep(5)
right_gripper.close()
rospy.sleep(5)
right_gripper.open()
def __init__(self):
self._pixel_meter_ratio = 0.001
self._threshold = 5
self.harmonic_repeat = float(2)
print('1')
rospy.wait_for_service('compute_ik')
print('2')
rospy.init_node('service_query')
print('3')
#Create the function used to call the service
self.compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
print('4')
self.right_gripper = robot_gripper.Gripper('right_gripper')
print('5')
self.limb = Limb()
print('6')
def __init__(self, limb, start_pose, hover_distance=0.15):
self.start_pose = start_pose
self.hover_distance = hover_distance
self.limb = robot_limb.Limb(limb)
self.gripper = robot_gripper.Gripper(limb)
self.base = "base_marker"
self.pick_obj = "obj_marker"
self.dest = "dest_marker"
self.destinations = []
self.queue = Queue.Queue()
self.tf = TransformListener()
self.set_scene()
self.set_limb_planner()
self.enable_robot()
self.set_destinations()
print("Moving Sawyer Arm to Start Position")
self.move_to_start()
#!/usr/bin/env python
import rospy
# from baxter_interface import gripper as robot_gripper
from intera_interface import gripper as robot_gripper
rospy.init_node('gripper_test')
#Set up the right gripper
right_gripper = robot_gripper.Gripper('right')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
right_gripper.calibrate()
rospy.sleep(2.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(5.0)
print('Done!')
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
# rospy.init_node('gripper_test')
#Set up the right gripper
right_gripper = robot_gripper.Gripper('right')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
right_gripper.calibrate()
rospy.sleep(2.0)
init_pos = [0.581, -0.423, -0.382]
final_pos = [0.603, -0.164, -0.367]
# #Close the right gripper
# print('Closing...')
# right_gripper.close()
# rospy.sleep(1.0)
# #Open the right gripper
# print('Opening...')
# right_gripper.open()
# rospy.sleep(1.0)
# print('Done!')
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = init_pos[0]
request.ik_request.pose_stamped.pose.position.y = init_pos[1]
request.ik_request.pose_stamped.pose.position.z = init_pos[2]
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander("right_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
#group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
rospy.sleep(1.0)
# #Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
# #Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = final_pos[0]
request.ik_request.pose_stamped.pose.position.y = final_pos[1]
request.ik_request.pose_stamped.pose.position.z = final_pos[2]
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander("right_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
#group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
rospy.sleep(1.0)
# #Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def main():
"""
Main Script
"""
#===================================================
# Code to add gripper
#rospy.init_node('gripper_test')
#Set up the right gripper
right_gripper = robot_gripper.Gripper('right_gripper')
#Calibrate the gripper (other commands won't work unless you do this first)
# print('Calibrating...')
# right_gripper.calibrate()
# rospy.sleep(2.0)
#Close the right gripper to hold publisher
# print('Closing...')
# right_gripper.close()
# rospy.sleep(1.0)
#===================================================
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
#Tower
#TODO: make wrt to sawyer (currently wrt to ar tag)
X = 0.075
Y = 0.075
Z = 0.045
Xp = 0.0
Yp = -0.0425
Zp = 0.0225
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
# pose = PoseStamped()
# pose.header.stamp = rospy.Time.now()
# #TODO: is this the right frame name?
# pose.header.frame_id = "ar_marker_1"
# pose.pose.position.x = Xp
# pose.pose.position.y = Yp
# pose.pose.position.z = Zp
# pose.pose.orientation.x = Xpa
# pose.pose.orientation.y = Ypa
# pose.pose.orientation.z = Zpa
# pose.pose.orientation.w = Wpa
# planner.add_box_obstacle([X,Y,Z], "tower", pose)
#Table (currently wrt ar tag)
X = 1
Y = 1
Z = .005
Xp = 0
Yp = 0
Zp = 0
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
#TODO: Is this the correct frame name?
pose.header.frame_id = "ar_marker_1"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
planner.add_box_obstacle([X, Y, Z], "table", pose)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper_tip"
#changed from "base" frame_id
orien_const.header.frame_id = "ar_marker_1"
orien_const.orientation.w = 1
orien_const.absolute_x_axis_tolerance = 0.001
orien_const.absolute_y_axis_tolerance = 0.001
orien_const.absolute_z_axis_tolerance = 0.001
orien_const.weight = 20.0
rospy.sleep(1.0)
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
#currently wrt ar tag
goal_1 = PoseStamped()
goal_1.header.frame_id = "ar_marker_1"
#x, y, and z position
goal_1.pose.position.x = 0.0
goal_1.pose.position.y = -0.0425
goal_1.pose.position.z = 0.0225
#Orientation as a quaternion
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = np.pi / 2
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 0
plan = planner.plan_to_pose(goal_1, list())
# if not planner.execute_plan(plan):
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_node')
#Set up the right gripper
right_gripper = robot_gripper.Gripper('right')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
right_gripper.calibrate()
rospy.sleep(2.0)
#Initialize both arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
# left_arm = moveit_commander.MoveGroupCommander('left_arm')
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# left_arm.set_planner_id('RRTConnectkConfigDefault')
# left_arm.set_planning_time(10)
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(10)
#First goal pose ------------------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.2
goal_1.pose.position.y = -0.5
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_1)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input(
'Press <Enter> to move the right arm to goal pose 1 (path constraints are never enforced during this motion): '
)
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_2)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the right arm to goal pose 2: ')
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
#Third goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_3)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the right arm to goal pose 3: ')
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
