def grip_calibrate():
print "Calibrating grippers..."
grip_l = gripper.Gripper("left")
grip_r = gripper.Gripper("right")
grip_l.calibrate()
grip_r.calibrate()
print "Finished calibrating"
class OrganizeAction(object):
rospy.init_node('motion_organizer')
# create messages that are used to publish feedback/result
_feedback = baxterkitchen.msg.OrganizeFeedback()
_result = baxterkitchen.msg.OrganizeResult()
#Set up the grippers
left_gripper = baxter_gripper.Gripper('left')
right_gripper = baxter_gripper.Gripper('right')
#Calibrate the grippers (other commands won't work unless you do this first)
print('Calibrating...')
left_gripper.calibrate()
right_gripper.calibrate()
# Set the force limit on the grippers
force_limit = 60 # approx 10N: 100 = 30N
left_gripper.set_moving_force(force_limit)
right_gripper.set_moving_force(force_limit)
left_gripper.set_holding_force(force_limit)
right_gripper.set_holding_force(force_limit)
rospy.sleep(1.0)
def __init__(self, name):
self._action_name = name
# add table to scene
self._as = actionlib.SimpleActionServer(
self._action_name,
baxterkitchen.msg.OrganizeAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
result = pick_client(0, 0.85, 0.25, 0)
result = pick_client(1, 0.85, -0.25, 0)
while True:
rospy.sleep(1)
def execute_cb(self,
goal): ###################### do action in this execute_cb
# helper variables
r = rospy.Rate(1)
success = True
#####################################################################################
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
# publish the feedback
self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self._result.done = success
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def main():
left = baxter_interface.Limb('left')
lj = left.joint_names()
left_gripper = robot_gripper.Gripper('left')
right = baxter_interface.Limb('right')
rj = right.joint_names()
right_gripper = robot_gripper.Gripper('right')
simult_gripper_close(left_gripper, right_gripper)
def main():
rospy.init_node("gripper_close")
right_gripper = robot_gripper.Gripper("right")
left_gripper = robot_gripper.Gripper("left")
left_gripper.calibrate()
right_gripper.calibrate()
rospy.sleep(2.0)
right_gripper.close()
rospy.sleep(0.1)
left_gripper.close()
def main():
startX, startY, startZ = 0.846, 0.215, -0.161
endX, endY, endZ = 0.794, 0.014, -0.149
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
left_gripper = robot_gripper.Gripper('left')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating left gripper...')
left_gripper.calibrate()
rospy.sleep(2.0)
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
move_to(startX, startY, startZ)
#Close the right gripper
print('Closing gripper...')
left_gripper.close()
rospy.sleep(1.0)
move_to(endX, endY, endZ)
#Open the right gripper
print('Opening left gripper...')
left_gripper.open()
rospy.sleep(1.0)
print('Done!')
def main():
# Initiate node, listener, and gripper
rospy.init_node("testing")
listener = tf.TransformListener()
right_gripper = robot_gripper.Gripper('right')
while not rospy.is_shutdown():
right_gripper.open()
try:
# Create and initialize the MoveGroup
group = MoveGroupCommander("right_arm")
init_move_group(group, position_tolerance=0.01)
print group.get_end_effector_link()
# for dbugging
#################
# end_location = get_artag_location(listener, "ar_marker_20")
# upper_left = get_artag_location(listener, "ar_marker_23")
# lower_right = get_artag_location(listener, "ar_marker_22")
# cg = RobotCheckers(upper_left, lower_right)
# end_location = cg.location(1, 1)
# print end_location
#################
# move = cg.detect_opponent_move(listener)
# print move
# move_group_to(group, 0.609, -0.131, -0.157)
move_group_to(group, 0.406, -0.140, -0.188)
# move_checkers_piece(group, right_gripper, listener, 0, end_location=end_location)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
sys.exit()
def main():
global left_arm
global left_gripper
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_interface')
#Set up Baxter Arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
left_arm = moveit_commander.MoveGroupCommander('left_arm')
left_arm.set_planner_id('RRTConnectkConfigDefault')
left_arm.set_planning_time(10)
#Calibrate the end effector on the left arm
left_gripper = baxter_gripper.Gripper('left')
print('Calibrating...')
left_gripper.calibrate()
rospy.sleep(2.0)
print('Ready to go')
#make subscriber to position topic
rospy.Subscriber('new_position', Pose, move_arm)
#make subscriber to gripper topic
rospy.Subscriber('gripper_control', Bool, actuate_gripper)
rospy.spin()
def GripperSetup():
global left_gripper
left_gripper = robot_gripper.Gripper('left')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating Gripper...')
left_gripper.calibrate()
rospy.sleep(2.0)
def main():
# rospy.spin()
left = baxter_interface.Limb('left')
lj = left.joint_names()
left_gripper = robot_gripper.Gripper('left')
right = baxter_interface.Limb('right')
rj = right.joint_names()
right_gripper = robot_gripper.Gripper('right')
print(right_gripper.force())
left.set_joint_position_speed(0.8)
right.set_joint_position_speed(0.8)
print("Initializing planner")
planner_right = PathPlanner("right_arm")
planner_left = PathPlanner("left_arm")
print("Calling function")
# move_at_angle(left, right, lj, rj, planner_left, planner_right, 0.4)
return_to_neutral(left, right, lj, rj, planner_left, planner_right)
def setup_gripper():
# Set up the gripper
gripper = robot_gripper.Gripper('left')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating Gripper...')
gripper.calibrate()
#Close the right gripper
print('Closing Gripper...')
gripper.close()
def move_hand(robot_hand, human_hand):
#Initiaize other variables
right_gripper = robot_gripper.Gripper('right')
left_gripper = robot_gripper.Gripper('left')
#right_gripper.calibrate()
right_gripper.close()
#Move to outside position
#Move right hand to outside position
raw_input('Press [Enter]:')
#Construct the request for right hand
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 500
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 = 0.5
request.ik_request.pose_stamped.pose.position.y = -0.35
request.ik_request.pose_stamped.pose.position.z = 0.4
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:
group = MoveGroupCommander("right_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
print("Trying to move the right arm out of the way!")
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def __init__(self):
print("Starting init")
self.left_arm = baxter_interface.Limb('left')
print("Left arm obtrained")
self.lj = self.left_arm.joint_names()
self.left_gripper = robot_gripper.Gripper('left')
self.left_joint_neutral = self.left_arm.joint_angles()
self.right_arm = baxter_interface.Limb('right')
self.rj = self.right_arm.joint_names()
self.right_gripper = robot_gripper.Gripper('right')
self.right_joint_neutral = self.right_arm.joint_angles()
self.planner_left = PathPlanner('left_arm')
self.planner_right = PathPlanner('right_arm')
self.orien_const_left_vert = OrientationConstraint()
self.orien_const_left_vert.link_name = "left_gripper"
self.orien_const_left_vert.header.frame_id = "base"
self.orien_const_left_vert.orientation.y = -1.0
self.orien_const_left_vert.absolute_x_axis_tolerance = 0.1
self.orien_const_left_vert.absolute_y_axis_tolerance = 0.1
self.orien_const_left_vert.absolute_z_axis_tolerance = 0.1
self.orien_const_left_vert.weight = 1.0
self.orien_const_right_vert = copy.deepcopy(self.orien_const_left_vert)
self.orien_const_right_vert.link_name = "right_gripper"
self.previous_deviation = (None, None)
self.setting_to_neutral = False
self.tilt_after_neutral = True
self.pub = rospy.Publisher("/board_controller_log",
std_msgs.msg.String,
queue_size=10)
rospy.Subscriber("/physics_inference", std_msgs.msg.String,
self.perform_tilt)
rospy.Subscriber("/control/neutral_set", std_msgs.msg.Bool,
self.neutral_listener)
rospy.Subscriber("control/tilt_set", std_msgs.msg.Bool,
self.tilt_listener)
def grip_control(hand, cl_op):
#Close = 1, open = 0
while True:
try:
grip = gripper.Gripper(hand)
break
except Exception, e:
print("ERROR, can not connect to gripper!")
print e
rospy.sleep(1.0)
print hand
print("Attempt reconnecting...")
def main():
rospy.init_node('BlackJack_dealer')
# INITIAL SETUP FOR VISION
# use right hand to take photos of cards
global image_topic
image_topic = "/cameras/right_hand_camera/image"
# train card recognition, only once
training()
# INITIAL SETUP FOR POSITION POLLING
global position_topic
position_topic = "/robot/limb/right/endpoint_state"
global right_x, right_y, right_z
# INITIAL SETUP FOR MOVEMENT
moveit_commander.roscpp_initialize(sys.argv)
# Initialize 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)
# Set up grippers
right_gripper = robot_gripper.Gripper('right')
left_gripper = robot_gripper.Gripper('left')
left_gripper.calibrate()
play = True
while play:
right_tuck(right_arm)
left_tuck(left_arm)
right_hover_deck(right_arm)
reset()
reset2()
gameplay(right_arm, right_gripper, left_arm, left_gripper)
answer = raw_input("Would you like to play another hand?\n")
if answer.lower() == "no":
play = False
def __init__(self, gui):
self.gripper = {
"left": gripper.Gripper("left"),
"right": gripper.Gripper("right")
}
self.ui = gui.ui
self.ui.btn_gripper_left_calibrate.clicked.connect(
lambda: self.gripper["left"].calibrate(False))
self.ui.btn_gripper_right_calibrate.clicked.connect(
lambda: self.gripper["right"].calibrate(False))
self.ui.btn_gripper_left_open.clicked.connect(
lambda: self.gripper["left"].open(False))
self.ui.btn_gripper_left_close.clicked.connect(
lambda: self.gripper["left"].close(False))
self.ui.btn_gripper_right_open.clicked.connect(
lambda: self.gripper["right"].open(False))
self.ui.btn_gripper_right_close.clicked.connect(
lambda: self.gripper["right"].close(False))
self.ui.btn_gripper_left_test.clicked.connect(
lambda: self.evaluateGripper("left"))
self.ui.btn_gripper_right_test.clicked.connect(
lambda: self.evaluateGripper("right"))
def __init__(self):
"""The constuctor of the TowerBuildingGame class.
Returns:
TowerBuildingGame: A new instance of the class TowerBuildingGame.
"""
##### ROS stuff #####
# Initializing AR_tag transforms
self.listener = tf.TransformListener()
# Verifying that robot is enabled
self._robotState = baxter_interface.RobotEnable(CHECK_VERSION)
self._initState = self._robotState.state().enabled
self._robotState.enable()
print("Robot is enabled.")
# Initializing the arm and the gripper
self.arm = baxter_interface.Limb(ARM_TO_USE)
self.gripper = robot_gripper.Gripper(ARM_TO_USE)
# Initializing the moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
##### Game logic stuff #####
self.isRunning = True
self.turn = 0 # Baxter: 0; Human: 1 (UNUSED AT THE MOMENT!)
self.playAlone = False
# Creating a Tower object which will represent the tower built
self.tower = Tower()
# Setting the default tower scanning position and orientation
self.towerScanPosition = [0.7, 0.0, 0.0]
self.towerScanOrientation = [0, -1, 0, 0]
# Setting the default warehouse scanning position and orientation
self.warehouseScanPosition = [0.7, 0.3, 0.0]
self.warehouseScanOrientation = [0, -1, 0, 0]
# Creating placeholders for the measurement of the reference block
# after the first measurement and at the beginnging of every turn of
# Baxter
self.refBlock_firstMeas = Block(REF_BLOCK_ID)
self.refBlock = Block(REF_BLOCK_ID)
# A placeholder for the block which Baxter decides to pick up next
self.nextBlock = None
print("Game instance is initialized successfully.")
def __init__(self, sub_topic1, sub_topic2):
self.ready = False
self.rate = rospy.Rate(10)
# self.queue = collections.deque(maxlen = 10)
# Generalize this to both arms?
self.group = moveit_commander.MoveGroupCommander("right_arm")
self.listener = tf.TransformListener()
self.gripper = robot_gripper.Gripper('right')
self.gripper.set_velocity(100)
self.gripper.calibrate()
rospy.sleep(2)
self.gripper.open()
self.sub_ready = rospy.Subscriber(sub_topic1, Bool, self.check_ready)
self.sub_ball_pos = rospy.Subscriber(sub_topic2, PointStamped,
self.grip)
def main():
# Initiate node, listener, and gripper
rospy.init_node("testing")
listener = tf.TransformListener()
right_gripper = robot_gripper.Gripper('right')
while not rospy.is_shutdown():
try:
tags = []
for i in range(12):
tags.append(get_artag_location(listener, "ar_marker_%d" % i))
# print all(tags)
if all(tags):
trans, rot = listener.lookupTransform("base", "left_gripper",
rospy.Time(0))
print trans, rot
sys.exit()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def keyboard_control(x):
#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)
left_gripper = robot_gripper.Gripper('left')
global step
step = 0
pose = [0.10, 0.20, 0.00,0.783, -0.621, 0.042, -0.004]
split = 10
def setGripper(self):
right_gripper = robot_gripper.Gripper('right')
print('Calibrating gripper')
right_gripper.calibrate()
rospy.sleep(2.0)
print('Opening gripper')
right_gripper.open()
rospy.sleep(1.0)
raw_input('Press <Enter> to close gripper: ')
right_gripper.command_position(self.gripDist)
right_gripper.set_holding_force(self.gripForce)
right_gripper.set_moving_force(self.gripForce)
# right_gripper.set_dead_band(45)
rospy.sleep(1.0)
print('Done!')
return
def map_user_input():
left = baxter_interface.Limb('left')
lj = left.joint_names()
left_gripper = robot_gripper.Gripper('left')
def set_j(limb, joint_names, joint_angles):
joint_command = {}
for i in range(len(joint_names)):
joint_command[joint_names[i]] = joint_angles[i]
limb.set_joint_positions(joint_command)
return joint_command
done = False
while not done and not rospy.is_shutdown():
print(lj)
user_joint_angles = raw_input(
"Enter 7 joint angles in comma-delimited format (e.g. 0.0,0.1,-1.2,1,1,0.1,-3.1): "
)
if user_joint_angles == "exit":
done = True
rospy.signal_shutdown("Example finished.")
elif len(user_joint_angles.split(",")) != 7:
print("7 joint angles were not passed in properly.")
continue
else:
joint_angles = [
float(angle) for angle in user_joint_angles.split(",")
]
# left_gripper.open()
rospy.sleep(0.5)
current_joint_angles = left.joint_angles()
for i in range(len(joint_angles)):
joint_angles[i] += current_joint_angles[lj[i]]
print(joint_angles)
commands = set_j(left, lj, joint_angles)
left_gripper.close()
print("New positions: ", commands)
rospy.sleep(0.01)
def __init__(self, limb):
rospy.init_node('regrasping_' + limb)
# Set up gripper
self.gripper = baxter_gripper.Gripper(limb)
self.gripper.calibrate()
# Set up limb
self.limb = baxter_interface.Limb(limb)
self.limb_name = limb
# Set up tf stuff
self.parent_frame = 'base'
self.listener = tf.TransformListener()
# Set up IK solver
self.ns = "ExternalTools/" + self.limb_name + "/PositionKinematicsNode/IKService"
self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK)
self.rate = rospy.Rate(100)
rospy.sleep(1.0)
def __init_moveit(self):
print "initilizing"
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
print "node"
#Start a node
print "left_gripper"
#Set up the left gripper
left_gripper = baxter_gripper.Gripper('left')
#Calibrate the gripper
# print('Calibrating...')
# left_gripper.calibrate()
# rospy.sleep(2.0)
#Initialize left arm
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.left_arm = moveit_commander.MoveGroupCommander('left_arm')
self.left_arm.set_planner_id('RRTConnectkConfigDefault')
self.left_arm.set_planning_time(10)
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
plannerR = PathPlanner("right_arm")
plannerL = PathPlanner("left_arm")
right_gripper = robot_gripper.Gripper('right')
left_gripper = robot_gripper.Gripper('left')
# 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
# right_arm = Limb("right")
# controller = Controller(Kp, Kd, Ki, Kw, right_arm)
##
## Add the obstacle to the planning scene here
##
obstacle1 = PoseStamped()
obstacle1.header.frame_id = "base"
#x, y, and z position
obstacle1.pose.position.x = 0.4
obstacle1.pose.position.y = 0
obstacle1.pose.position.z = -0.29
#Orientation as a quaternion
obstacle1.pose.orientation.x = 0.0
obstacle1.pose.orientation.y = 0.0
obstacle1.pose.orientation.z = 0.0
obstacle1.pose.orientation.w = 1
plannerR.add_box_obstacle(np.array([1.2, 1.2, .005]), "tableBox",
obstacle1)
# obstacle2 = PoseStamped()
# obstacle2.header.frame_id = "wall"
# #x, y, and z position
# obstacle2.pose.position.x = 0.466
# obstacle2.pose.position.y = -0.670
# obstacle2.pose.position.z = -0.005
# #Orientation as a quaternion
# obstacle2.pose.orientation.x = 0.694
# obstacle2.pose.orientation.y = -0.669
# obstacle2.pose.orientation.z = 0.251
# obstacle2.pose.orientation.w = -0.092
# planner.add_box_obstacle(np.array([0.01, 2, 2]), "wall", obstacle2)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# 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;
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.440
goal_3.pose.position.y = -0.012
goal_3.pose.position.z = 0.549
#Orientation as a quaternion
goal_3.pose.orientation.x = -0.314
goal_3.pose.orientation.y = -0.389
goal_3.pose.orientation.z = 0.432
goal_3.pose.orientation.w = 0.749
#plan = plannerR.plan_to_pose(goal_3, list())
raw_input(
"Press <Enter> to move the right arm to goal pose 3: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_3, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.448
goal_2.pose.position.y = -0.047
goal_2.pose.position.z = -0.245
#Orientation as a quaternion
goal_2.pose.orientation.x = 0
goal_2.pose.orientation.y = 1
goal_2.pose.orientation.z = 0
goal_2.pose.orientation.w = 0
#plan = plannerR.plan_to_pose(goal_2, list())
raw_input(
"Press <Enter> to move the right arm to goal pose 2: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_2, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.448
goal_1.pose.position.y = -0.047
goal_1.pose.position.z = -0.245
#Orientation as a quaternion
goal_1.pose.orientation.x = 1
goal_1.pose.orientation.y = 0
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 0
# Might have to edit this . . .
#plan = plannerR.plan_to_pose(goal_1, list()) # put orien_const here
raw_input(
"Press <Enter> to move the right arm to goal pose 1: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_1, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
# K values for Part 5
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3
]) # Borrowed from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Borrowed 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
# Initialize the controller for Part 5
# controller = Controller( . . . )
#-----------------------------------------------------#
## Add any obstacles to the planning scene here
#-----------------------------------------------------#
size = np.array([0.4, 1.2, 0.1])
pose = PoseStamped()
pose.header.frame_id = "base"
pose.pose.position.x = 0.5
pose.pose.position.y = 0.0
pose.pose.position.z = 0.0
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.z = 1.0
# planner.add_box_obstacle(size, "box", pose)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# 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;
def move_to_goal(x,
y,
z,
orien_const=[],
or_x=0.0,
or_y=-1.0,
or_z=0.0,
or_w=0.0):
while not rospy.is_shutdown():
try:
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
#Orientation as a quaternion
goal.pose.orientation.x = or_x
goal.pose.orientation.y = or_y
goal.pose.orientation.z = or_z
goal.pose.orientation.w = or_w
plan = planner.plan_to_pose(goal, orien_const)
# raw_input("Press <Enter> to move the right arm to goal pose: ")
rospy.sleep(1)
# Might have to edit this for part 5
if not planner.execute_plan(plan):
raise Exception("Execution failed")
else:
break
except Exception as e:
print e
traceback.print_exc()
# else:
# break
while not rospy.is_shutdown():
right_gripper = robot_gripper.Gripper('right')
right_gripper.set_moving_force(80.0)
right_gripper.set_holding_force(80.0)
# right_gripper.calibrate()
# Set your goal positions here
print("starting")
# move_to_goal(0.85, -0.3001, 0.1)
# rospy.sleep(1.)
move_to_goal(0.85, -0.2995, 0.1)
print("opening")
right_gripper.open()
rospy.sleep(1.)
print("executing")
move_to_goal(0.85, -0.2995, -0.030) #-0.41
print("closings")
# right_gripper.close()
print("MISSED: ", right_gripper.missed())
print("FORCEEE: ", right_gripper.force())
print("Done")
# move_to_goal(0.4225 + 0.1, -0.1265, 0.7725 - 0.92)
# right_gripper.close()
# move_to_goal(0.4225 + 0.1 + 0.05, -0.1265, 0.7725 - 0.92)
# right_gripper.open()
break
def main():
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_node')
#Set up the left gripper
left_gripper = baxter_gripper.Gripper('left')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
left_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.6
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
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input(
'Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): '
)
left_arm.execute(left_plan)
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
print('Done!')
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.4
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
left_arm.set_pose_target(goal_2)
#Set the start state for the left arm
left_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 = "left_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]
left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 2: ')
left_arm.execute(left_plan)
#Third goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
#print('Done!')
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.0
goal_3.pose.position.y = 0.7
goal_3.pose.position.z = 0.0
#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
left_arm.set_pose_target(goal_3)
#Set the start state for the left arm
left_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 = "left_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]
left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 3: ')
left_arm.execute(left_plan)
#Fourth goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
#print('Done!')
goal_4 = PoseStamped()
goal_4.header.frame_id = "base"
#x, y, and z position
goal_4.pose.position.x = 0.4
goal_4.pose.position.y = 0.7
goal_4.pose.position.z = 0.3
#Orientation as a quaternion
goal_4.pose.orientation.x = 0.0
goal_4.pose.orientation.y = -1.0
goal_4.pose.orientation.z = 0.0
goal_4.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_4)
#Set the start state for the left arm
left_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 = "left_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]
#left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 4: ')
left_arm.execute(left_plan)
rospy.sleep(2.0)
#Close the left gripper
print('Closing...')
left_gripper.close(block=True)
rospy.sleep(0.5)
#Open the left gripper
print('Opening...')
left_gripper.open(block=True)
rospy.sleep(1.0)
print('Done!')
MESH_FILENAME = '/home/cc/ee106b/sp18/class/ee106b-aax/ros_workspaces/lab2_ws/src/lab2_pkg/objects/gearbox.obj'
TAG = 14
T_ar_object = tfs.translation_matrix([-.09, .065, 0.058])
SUBDIVIDE_STEPS = 0
if __name__ == '__main__':
if BAXTER_CONNECTED:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_node')
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planner_id('SPARSConnectkConfigDefault')
right_arm.set_planning_time(15)
right_gripper = baxter_gripper.Gripper('right')
right_gripper.calibrate()
listener = tf.TransformListener()
from_frame = 'base'
time.sleep(1)
# Main Code
br = tf.TransformBroadcaster()
# SETUP
mesh = trimesh.load(MESH_FILENAME)
vertices = mesh.vertices
triangles = mesh.triangles
normals = mesh.vertex_normals
of = ObjFile(MESH_FILENAME)
def main(list_of_class_objs, basket_coordinates, planner_left):
"""
Main Script
input:first argument: a list of classX_objs. Ex: [class1_objs, class2_objs] where class1_objs contains the same kind of fruits
second argument: a list of baskets coordinates
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner_left = PathPlanner("left_arm")
home_coordinates = [0.544, 0.306, 0.3]
home = PoseStamped()
home.header.frame_id = "base"
#home x,y,z position
home_x = home_coordinates[0]
home_y = home_coordinates[1]
home_z = home_coordinates[2]
home.pose.position.x = home_x
home.pose.position.y = home_y
home.pose.position.z = home_z
#Orientation as a quaternion
home.pose.orientation.x = 1.0
home.pose.orientation.y = 0.0
home.pose.orientation.z = 0.0
home.pose.orientation.w = 0.0
intermediate_obstacle = PoseStamped()
intermediate_obstacle.header.frame_id = "base"
intermediate_obstacle.pose.position.x = 0
intermediate_obstacle.pose.position.y = 0
intermediate_obstacle.pose.position.z = 0.764
intermediate_obstacle.pose.orientation.x = 1
intermediate_obstacle.pose.orientation.y = 0
intermediate_obstacle.pose.orientation.z = 0
intermediate_obstacle.pose.orientation.w = 0
intermediate_size = [1, 1, 0.2]
left_gripper = robot_gripper.Gripper('left')
print('Calibrating...')
left_gripper.calibrate()
while not rospy.is_shutdown():
try:
#GO HOME
plan = planner_left.plan_to_pose(home, list())
raw_input("Press <Enter> to move the left arm to home position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
for i, classi_objs in enumerate(list_of_class_objs):
#x, y,z, orientation of class(basket)
print("processing class: " + str(i))
classi_position = basket_coordinates[i]
classi = PoseStamped()
classi.header.frame_id = "base"
classi_x = classi_position[0]
classi_y = classi_position[1]
classi_z = classi_position[2]
classi.pose.position.x = classi_x
classi.pose.position.y = classi_y
classi.pose.position.z = classi_z +.1
classi.pose.orientation.x = 1.0
classi.pose.orientation.y = 0.0
classi.pose.orientation.z = 0.0
classi.pose.orientation.w = 0.0
for fruit in classi_objs:
gripper_yaw = fruit[3]
fruit_x = fruit[0]
fruit_y = fruit[1]
fruit_z = fruit[2]
gripper_orientation = euler_to_quaternion(gripper_yaw)
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
gripper_orientation_x = gripper_orientation[0]
gripper_orientation_y = gripper_orientation[1]
gripper_orientation_z = gripper_orientation[2]
gripper_orientation_w = gripper_orientation[3]
orien_const.orientation.x = gripper_orientation_x
orien_const.orientation.y = gripper_orientation_y
orien_const.orientation.z = gripper_orientation_z
orien_const.orientation.w = gripper_orientation_w
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
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose)
#intermidiate_to_fruit stage: move to the top of the fruit location and open the gripper
intermidiate_to_fruit = PoseStamped()
intermidiate_to_fruit.header.frame_id = "base"
#x,y,z position
intermidiate_to_fruit.pose.position.x = fruit_x
intermidiate_to_fruit.pose.position.y = fruit_y
intermidiate_to_fruit.pose.position.z = home_z - .1
print("Trying to reach intermeidiate_to_fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z))
intermidiate_to_fruit.pose.orientation.x = gripper_orientation_x
intermidiate_to_fruit.pose.orientation.y = gripper_orientation_y
intermidiate_to_fruit.pose.orientation.z = gripper_orientation_z
intermidiate_to_fruit.pose.orientation.w = gripper_orientation_w
plan = planner_left.plan_to_pose(intermidiate_to_fruit, list())
raw_input("Press <Enter> to move the left arm to intermidiate_to_fruit position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
planner_left.remove_obstacle("intermediate")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
#go down to the actual height of the fruit and close gripper
fruitobs = generate_obstacle(fruit_x, fruit_y)
fruit = PoseStamped()
fruit.header.frame_id = "base"
#x,y,z position
fruit.pose.position.x = fruit_x
fruit.pose.position.y = fruit_y
fruit.pose.position.z = fruit_z
print("Trying to reach fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z))
#Orientation as a quaternion
fruit.pose.orientation.x = gripper_orientation_x
fruit.pose.orientation.y = gripper_orientation_y
fruit.pose.orientation.z = gripper_orientation_z
fruit.pose.orientation.w = gripper_orientation_w
plan = planner_left.plan_to_pose(fruit, [orien_const])
raw_input("Press <Enter> to move the left arm to fruit position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
fruitobs()
except Exception as e:
print e
else:
break
#close the gripper
print('Closing...')
left_gripper.close()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
fruitobs = generate_obstacle(fruit_x, fruit_y)
#intermidiate_to_basket stage1: Lift up to a height higher than the height of the basket
firt_intermidiate_to_class_i = PoseStamped()
firt_intermidiate_to_class_i.header.frame_id = "base"
#x, y, and z position
firt_intermidiate_to_class_i.pose.position.x = fruit_x
firt_intermidiate_to_class_i.pose.position.y = fruit_y
firt_intermidiate_to_class_i.pose.position.z = classi_z + 0.25
print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2))
#Orientation as a quaternion
firt_intermidiate_to_class_i.pose.orientation.x = 1.0
firt_intermidiate_to_class_i.pose.orientation.y = 0.0
firt_intermidiate_to_class_i.pose.orientation.z = 0.0
firt_intermidiate_to_class_i.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(firt_intermidiate_to_class_i, list())
raw_input("Press <Enter> to move the left arm to first_intermidiate_to_class_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
fruitobs()
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose)
#intermidiate_to_basket stage2: Move to the top of the basket
intermidiate_to_class_i = PoseStamped()
intermidiate_to_class_i.header.frame_id = "base"
#x, y, and z position
intermidiate_to_class_i.pose.position.x = classi_x
intermidiate_to_class_i.pose.position.y = classi_y
intermidiate_to_class_i.pose.position.z = classi_z + 0.2
print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2))
#Orientation as a quaternion
intermidiate_to_class_i.pose.orientation.x = 1.0
intermidiate_to_class_i.pose.orientation.y = 0.0
intermidiate_to_class_i.pose.orientation.z = 0.0
intermidiate_to_class_i.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(intermidiate_to_class_i, list())
raw_input("Press <Enter> to move the left arm to second_intermidiate_to_class_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
planner_left.remove_obstacle("intermediate")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
#basket stage: put the fruit in the basket
classi_obs = generate_obstacle(classi_x, class_y)
plan = planner_left.plan_to_pose(classi, list())
raw_input("Press <Enter> to move the left arm to sclass_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
classi_obs()
except Exception as e:
print e
else:
break
#Open the gripper
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
#intermidiate to home stage: lifting up to the home position height
intermidiate_to_home_1 = PoseStamped()
intermidiate_to_home_1.header.frame_id = "base"
#x, y, and z position
intermidiate_to_home_1.pose.position.x = classi_x
intermidiate_to_home_1.pose.position.y = classi_y
intermidiate_to_home_1.pose.position.z = home_z
#Orientation as a quaternion
intermidiate_to_home_1.pose.orientation.x = 1.0
intermidiate_to_home_1.pose.orientation.y = 0.0
intermidiate_to_home_1.pose.orientation.z = 0.0
intermidiate_to_home_1.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(intermidiate_to_home_1, list())
raw_input("Press <Enter> to move the right arm to intermidiate_to_home_1 position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
planner_left.shutdown()
from intera_interface import gripper as robot_gripper
from intera_interface import Limb
from std_msgs.msg import String
import moveit_commander
import rospy
import sys
from copy import deepcopy
import tf
import pdb
rospy.init_node('default_position')
pose = None
moveit_commander.roscpp_initialize(sys.argv)
right_gripper = robot_gripper.Gripper('right')
robot = moveit_commander.RobotCommander()
right_arm = moveit_commander.MoveGroupCommander('right_arm')
right_arm.set_planning_time(20)
def planning():
rate = rospy.Rate(10) # 10hz
limb = Limb('right')
default_joints = {
'head_pan': -4.2551240234375,
'right_j0': -2.3731005859375,
'right_j1': -2.4028828125,
'right_j2': 1.658787109375,
'right_j3': 0.7297041015625,
class OrganizeAction(object):
rospy.init_node('motion_organizer')
# create messages that are used to publish feedback/result
_feedback = baxterkitchen.msg.OrganizeFeedback()
_result = baxterkitchen.msg.OrganizeResult()
sub = rospy.Subscriber('inventory', Inventory, sub_callback, queue_size=10)
#Set up the grippers
left_gripper = baxter_gripper.Gripper('left')
right_gripper = baxter_gripper.Gripper('right')
#Calibrate the grippers (other commands won't work unless you do this first)
print('Calibrating...')
left_gripper.calibrate()
right_gripper.calibrate()
# Set the force limit on the grippers
force_limit = 60 # approx 10N: 100 = 30N
left_gripper.set_moving_force(force_limit)
right_gripper.set_moving_force(force_limit)
left_gripper.set_holding_force(force_limit)
right_gripper.set_holding_force(force_limit)
# setup scene
p = PlanningSceneInterface("base")
#p.clear()
rospy.sleep(1.0)
def __init__(self, name):
self._action_name = name
#self.p.addBox("test", 2,4,6, 0,0,0)
# add table to scene
self.p.addBox("table", 0.75, 1.50, 0.94, 0.55, 0.0, -0.56)
#self.p.setColor("table",1,1,1,0.2)
self.p.addBox("knife", objectSize['knife'][0], objectSize['knife'][1],
objectSize['knife'][2],
knife_px + objectOffset['knife'][0],
knife_py + objectOffset['knife'][1],
knife_pz + objectOffset['knife'][2])
self.p.addBox("carrot", objectSize['carrot'][0],
objectSize['carrot'][1], objectSize['carrot'][2],
carrot_px + objectOffset['carrot'][0],
carrot_py + objectOffset['carrot'][1],
carrot_pz + objectOffset['carrot'][2])
self.p.addBox("corn", objectSize['corn'][0], objectSize['corn'][1],
objectSize['corn'][2], corn_px + objectOffset['corn'][0],
corn_py + objectOffset['corn'][1],
corn_pz + objectOffset['corn'][2])
self.p.addBox("dish", objectSize['dish'][0], objectSize['dish'][1],
objectSize['dish'][2], dish_px + objectOffset['dish'][0],
dish_py + objectOffset['dish'][1],
dish_pz + objectOffset['dish'][2])
self.p.addBox("sponge", objectSize['sponge'][0],
objectSize['sponge'][1], objectSize['sponge'][2],
sponge_px + objectOffset['sponge'][0],
sponge_py + objectOffset['sponge'][1],
sponge_pz + objectOffset['sponge'][2])
self.p.addBox("rack", objectSize['rack'][0], objectSize['rack'][1],
objectSize['rack'][2], rack_px + objectOffset['rack'][0],
rack_py + objectOffset['rack'][1],
rack_pz + objectOffset['rack'][2])
self._as = actionlib.SimpleActionServer(
self._action_name,
baxterkitchen.msg.OrganizeAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
def execute_cb(self,
goal): ###################### do action in this execute_cb
# helper variables
r = rospy.Rate(1)
success = True
#rospy.sleep(20)
##################################################################################
print 'motion org received request'
print goal.chore
print goal.target
print goal.reps
if goal.chore == 'untuck':
print 'untucking'
try:
print 'pick sponge'
result = untuck_client(1)
except rospy.ROSInterruptException:
print "program interrupted before completion"
if goal.chore == 'wash': ########### wash execution
print 'washing...'
try:
print 'pick sponge'
result = move_client(0, sponge_px, sponge_py, sponge_pz + 0.08)
self.p.removeCollisionObject('sponge')
result = pick_client(0, sponge_px, sponge_py, sponge_pz - 0.02)
result = move_client(0, sponge_px, sponge_py, sponge_pz + 0.10)
for times in range(0, int(goal.reps)):
print 'pick dish ', times
result = move_client(1, dish_px, dish_py, dish_pz + 0.10)
self.p.removeCollisionObject('dish')
result = pick_client(1, dish_px, dish_py, dish_pz - 0.015)
result = move_client(1, dish_px, dish_py, dish_pz + 0.10)
print 'move dish to location'
result = move_client(1, 0.75, -0.14, 0.255)
print 'scrub the dish...'
result = scrub_client(
0.0, 0.0, 0.0) # position input doesnt matter...
print "after scrub..."
print "return dish ", times
result = move_client(1, 0.774, -0.492, 0.18)
result = place_client(1, 0.774, -0.492, 0.12)
result = move_client(1, 0.774, -0.492, 0.18)
self.p.addBox("dish", objectSize['dish'][0],
objectSize['dish'][1], objectSize['dish'][2],
dish_px + objectOffset['dish'][0],
dish_py + objectOffset['dish'][1],
dish_pz + objectOffset['dish'][2])
print 'return sponge'
result = place_client(0, 0.6, 0.4, 0.05)
self.p.addBox("sponge", objectSize['sponge'][0],
objectSize['sponge'][1], objectSize['sponge'][2],
sponge_px + objectOffset['sponge'][0],
sponge_py + objectOffset['sponge'][1],
sponge_pz + objectOffset['sponge'][2])
print "Finished washing: ", result
except rospy.ROSInterruptException:
print "program interrupted before completion"
if goal.chore == 'cut': ############ cut execution
print 'cutting...'
try:
if goal.pick_tool == True:
print 'pick knife'
print knife_px
print knife_py
print knife_pz
result = move_client(0, knife_px - .03, knife_py,
knife_pz + 0.1)
self.p.removeCollisionObject('knife')
result = pick_client(0, knife_px, knife_py, knife_pz)
result = move_client(0, knife_px, knife_py, knife_pz + 0.1)
print 'pick ', goal.target
if goal.target == 'carrot':
print 'cut carrot'
result = move_client(1, 0.6, -0.6, 0.27)
result = move_client(1, carrot_px, carrot_py,
carrot_pz + 0.10)
self.p.removeCollisionObject('carrot')
result = pick_client(1, carrot_px + 0.01, carrot_py,
carrot_pz + 0.02)
result = move_client(1, carrot_px, carrot_py,
carrot_pz + 0.10)
else:
print 'cut corn'
self.p.removeCollisionObject('corn')
result = move_client(1, 0.6, -0.6, 0.27)
result = move_client(1, corn_px, corn_py, corn_pz + 0.10)
self.p.removeCollisionObject('corn')
result = pick_client(1, corn_px + 0.01, corn_py,
corn_pz + 0.02)
result = move_client(1, corn_px, corn_py, corn_pz + 0.10)
print 'move ', goal.target
result = move_client(1, 0.683, -0.024, -0.03)
print 'cut...', goal.reps
result = cut_client(int(
goal.reps)) # position input doesnt matter...
print 'move knife out of the way'
result = move_client(0, 0.505, 0.287, 0.014)
print 'return cut object'
result = place_client(1, 0.75, -0.12, -0.02)
result = move_client(1, 0.75, -0.12, 0.1)
if goal.target == 'carrot':
self.p.addBox("carrot", objectSize['carrot'][0],
objectSize['carrot'][1],
objectSize['carrot'][2],
carrot_px + objectOffset['carrot'][0],
carrot_py + objectOffset['carrot'][1],
carrot_pz + objectOffset['carrot'][2])
else:
self.p.addBox("corn", objectSize['corn'][0],
objectSize['corn'][1], objectSize['corn'][2],
corn_px + objectOffset['corn'][0],
corn_py + objectOffset['corn'][1],
corn_pz + objectOffset['corn'][2])
if goal.return_tool == True:
print 'return knife'
result = place_client(0, 0.5, 0.28, -0.05)
result = move_client(0, 0.5, 0.28, 0.05)
self.p.addBox("knife", objectSize['knife'][0],
objectSize['knife'][1],
objectSize['knife'][2],
knife_px + objectOffset['knife'][0],
knife_py + objectOffset['knife'][1],
knife_pz + objectOffset['knife'][2])
print "Finished cutting"
except rospy.ROSInterruptException:
print "program interrupted before completion"
#####################################################################################
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
self._as.set_preempted()
success = False
# publish the feedback
self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
if success:
self._result.done = success
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
