class track():
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper = Gripper("right")
self.gripper.calibrate()
self.gripper.set_moving_force(0.1)
rospy.sleep(0.5)
self.gripper.set_holding_force(0.1)
rospy.sleep(0.5)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
def _read_distance(self,data):
self.distance = data.range
def vision_request_right(self, controlID, requestID):
try:
rospy.wait_for_service('vision_server_vertical')
vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical)
return vision_server_req(controlID, requestID)
except (rospy.ServiceException,rospy.ROSInterruptException), e:
print "Service call failed: %s" % e
self.clean_shutdown_service()
class track():
def __init__(self, pr_init, pl_init):
self.centerx = 365
self.centery = 140
self.coefx = 0.1 / (526 - 369)
self.coefy = 0.1 / (237 - 90)
self.count = 0
self.right = MoveGroupCommander("right_arm")
self.left = MoveGroupCommander("left_arm")
self.gripper = Gripper("right")
self.gripper.calibrate()
self.gripper.set_moving_force(0.1)
self.gripper.set_holding_force(0.1)
self.pr = pr_init
self.pl = pl_init
self.angle = 0
self.position_list = (0.7, -0.3, -0.06, 0, 0.0, self.angle)
self.busy = False
self.blank_image = np.zeros((400, 640, 3), np.uint8)
cv2.putText(self.blank_image,
"DEMO", (180, 200),
cv2.FONT_HERSHEY_SIMPLEX,
fontScale=4,
color=255,
thickness=10)
self.movenum = 0
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.H = homography()
self.framenumber = 0
self.history = np.arange(0, 20) * -1
self.newPosition = True
self.bowlcamera = None
def vision_request_right(self, controlID, requestID):
rospy.wait_for_service('vision_server_vertical')
try:
vision_server_req = rospy.ServiceProxy('vision_server_vertical',
Vision)
return vision_server_req(controlID, requestID)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class track():
def __init__(self, pr_init, pl_init):
self.centerx = 365
self.centery = 140
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.right = MoveGroupCommander("right_arm")
self.left = MoveGroupCommander("left_arm")
self.gripper = Gripper("right")
self.gripper.calibrate()
self.gripper.set_moving_force(0.1)
self.gripper.set_holding_force(0.1)
self.pr = pr_init
self.pl = pl_init
self.angle = 0
self.position_list = (0.7,-0.3,-0.06,0,0.0,self.angle)
self.busy = False
self.blank_image = np.zeros((400,640,3),np.uint8)
cv2.putText(self.blank_image,"DEMO", (180,200), cv2.FONT_HERSHEY_SIMPLEX, fontScale = 4, color = 255, thickness = 10 )
self.movenum = 0
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.H = homography()
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
def vision_request_right(self, controlID, requestID):
rospy.wait_for_service('vision_server_vertical')
try:
vision_server_req = rospy.ServiceProxy('vision_server_vertical', Vision)
return vision_server_req(controlID, requestID)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class Baxter_Deli(object):
def __init__(self):
self.rightg = Gripper('right')
self.rightg.set_holding_force(100)
self.leftg = Gripper('left')
self.right = Limb('right')
self.left = Limb('left')
self.head = Head()
self.angles= list()
rospy.Subscriber("command", String, self.command)
rospy.spin()
def command(self, comm):
if comm.data == "left":
self.angles.append(self.left.joint_angles())
elif comm.data == "right":
self.angles.append(self.right.joint_angles())
elif comm.data == "done":
print self.angles
class GraspExecuter(object):
def __init__(self, max_vscale=1.0):
self.grasp_queue = Queue()
initialized = False
while not initialized:
try:
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.left_arm = moveit_commander.MoveGroupCommander('left_arm')
self.left_arm.set_max_velocity_scaling_factor(max_vscale)
self.go_to_pose('left', L_HOME_STATE)
self.right_arm = moveit_commander.MoveGroupCommander(
'right_arm')
self.right_arm.set_max_velocity_scaling_factor(max_vscale)
self.go_to_pose('right', R_HOME_STATE)
self.left_gripper = Gripper('left')
self.left_gripper.set_holding_force(GRIPPER_CLOSE_FORCE)
self.left_gripper.open()
self.right_gripper = Gripper('right')
self.left_gripper.set_holding_force(GRIPPER_CLOSE_FORCE)
self.right_gripper.open()
initialized = True
except rospy.ServiceException as e:
rospy.logerr(e)
def queue_grasp(self, req):
grasp = req.grasp
rotation_quaternion = np.asarray([
grasp.pose.orientation.w, grasp.pose.orientation.x,
grasp.pose.orientation.y, grasp.pose.orientation.z
])
translation = np.asarray([
grasp.pose.position.x, grasp.pose.position.y, grasp.pose.position.z
])
T_grasp_camera = RigidTransform(rotation_quaternion, translation,
'grasp', T_camera_world.from_frame)
T_gripper_world = T_camera_world * T_grasp_camera * T_gripper_grasp
self.grasp_queue.put(T_gripper_world.pose_msg)
return True
def execute_grasp(self, grasp):
if grasp.position.z < TABLE_DEPTH + 0.02:
grasp.position.z = TABLE_DEPTH + 0.02
approach = deepcopy(grasp)
approach.position.z = grasp.position.z + GRASP_APPROACH_DIST
# perform grasp on the robot, up until the point of lifting
rospy.loginfo('Approaching')
self.go_to_pose('left', approach)
# grasp
rospy.loginfo('Grasping')
self.go_to_pose('left', grasp)
self.left_gripper.close(block=True)
#lift object
rospy.loginfo('Lifting')
self.go_to_pose('left', approach)
# Drop in bin
rospy.loginfo('Going Home')
self.go_to_pose('left', L_PREGRASP_POSE)
self.left_gripper.open()
lift_gripper_width = self.left_gripper.position() # a percentage
# check drops
lifted_object = lift_gripper_width > 0.0
return lifted_object, lift_gripper_width
def go_to_pose(self, arm, pose):
"""Uses Moveit to go to the pose specified
Parameters
----------
pose : :obj:`geometry_msgs.msg.Pose` or RigidTransform
The pose to move to
max_velocity : fraction of max possible velocity
"""
if arm == 'left':
arm = self.left_arm
else:
arm = self.right_arm
arm.set_start_state_to_current_state()
arm.set_pose_target(pose)
arm.plan()
arm.go()
class track():
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper_left = Gripper("left")
self.gripper_left.calibrate()
self.gripper_left.set_moving_force(0.01)
rospy.sleep(0.5)
self.gripper_left.set_holding_force(0.01)
rospy.sleep(0.5)
self.gripper_right = Gripper("right")
self.gripper_right.calibrate()
rospy.sleep(1)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin_right = baxter_kinematics('right')
self.kin_left = baxter_kinematics('left')
self.J = self.kin_right.jacobian()
self.J_inv = self.kin_right.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.left_arm = Limb("left")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
self.pour_x = 0
self.pour_y = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
self.joint_states = {
'observe':{
'right_e0': -0.631,
'right_e1': 0.870,
'right_s0': 0.742,
'right_s1': -0.6087,
'right_w0': 0.508,
'right_w1': 1.386,
'right_w2': -0.5538,
},
'observe_ladle':{
'right_e0': -0.829,
'right_e1': 0.831,
'right_s0': 0.678,
'right_s1': -0.53,
'right_w0': 0.716,
'right_w1': 1.466,
'right_w2': -0.8099,
},
'observe_left':{
'left_w0': 2.761932405432129,
'left_w1': -1.5700293346069336,
'left_w2': -0.8893253607604981,
'left_e0': -0.9805972175354004,
'left_e1': 1.8300390778564455,
'left_s0': 1.4783739826354982,
'left_s1': -0.9503010970092775,
},
'stir':{
'right_e0': -0.179,
'right_e1': 1.403,
'right_s0': 0.381,
'right_s1': -0.655,
'right_w0': 1.3,
'right_w1': 2.04,
'right_w2': 0.612,
},
'observe_vertical':{
'right_e0': 0.699,
'right_e1': 1.451,
'right_s0': -1.689,
'right_s1': 0.516,
'right_w0': 0.204,
'right_w1': 0.935,
'right_w2': -2.706,
},
'observe_midpoint':{
'right_e0': -0.606,
'right_e1': 0.968,
'right_s0': 0.0,
'right_s1': -0.645,
'right_w0': 0.465,
'right_w1': 1.343,
'right_w2': -0.437,
},
'dressing':{
'right_e0': 0.967,
'right_e1': 1.386,
'right_s0': -0.348,
'right_s1': -0.155,
'right_w0': 0.264,
'right_w1': 1.521,
'right_w2': -2.199,
},
}
def _read_distance(self,data):
self.distance = data.range
def vision_request_right(self, controlID, requestID):
try:
rospy.wait_for_service('vision_server_vertical')
vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical)
return vision_server_req(controlID, requestID)
except (rospy.ServiceException,rospy.ROSInterruptException), e:
print "Service call failed: %s" % e
self.clean_shutdown_service()
#oatmeal raisin cookie( one ):
#gripper: narrow (left)
#holding force: 40
#replace cookie with drink
#individually wrapped triangle sandwich will not do
#!/usr/bin/env python
import rospy, baxter_interface
from baxter_interface import Gripper, Head, Limb
rospy.init_node('deli_Baxter')
rightg = Gripper('right')
rightg.set_holding_force(100)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
head.set_pan(0.0, speed=0.8, timeout=0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
neutral_right = {
'right_s0': 0.5971020216843973,
'right_s1': -0.4237621926533455,
'right_w0': 0.4226117070624315,
'right_w1': 0.8471408901097197,
'right_w2': -0.9725438195193523,
#!/usr/bin/env python
import sys
import time
import rospy
from std_msgs.msg import UInt32
from msg_types import *
import baxter_interface
from baxter_interface import Gripper, Head, Limb
rospy.init_node('run_condition')
voice_pub = rospy.Publisher('/voice', UInt32, queue_size=10)
time.sleep(1)
rightg = Gripper('right')
rightg.set_holding_force(50)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
neutral_right = {'right_s0': 0.476301034638421, 'right_s1': -0.5606699779721187, 'right_w0': 0.07094661143970038, 'right_w1': 0.7037136864424336, 'right_w2': -0.28033498898605935, 'right_e0': -0.16566992509162468, 'right_e1': 1.3077186216723151}
neutral_left = {'left_w0': -0.15339807878854136, 'left_w1': 0.34552917247118947, 'left_w2': 3.0158062289827234, 'left_e0': 0.18676216092504913, 'left_e1': 1.5715633171886063, 'left_s0': -0.5836796897904, 'left_s1': -0.6845389265938658}
right.move_to_joint_positions(neutral_right)
left.move_to_joint_positions(neutral_left)
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
#oatmeal raisin cookie( one ):
#gripper: narrow (left)
#holding force: 40
#replace cookie with drink
#individually wrapped triangle sandwich will not do
#!/usr/bin/env python
import rospy, baxter_interface
from baxter_interface import Gripper, Head, Limb
rospy.init_node('deli_Baxter')
rightg = Gripper('right')
rightg.set_holding_force(100)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
head.set_pan(0.0, speed = 0.8, timeout = 0.0)
Gripper.calibrate(rightg)
Gripper.calibrate(leftg)
neutral_right ={'right_s0': 0.5971020216843973, 'right_s1': -0.4237621926533455, 'right_w0': 0.4226117070624315, 'right_w1': 0.8471408901097197, 'right_w2': -0.9725438195193523, 'right_e0': -0.40727189918357737, 'right_e1': 1.161990446823201}
neutral_left = {'left_w0': -0.3006602344255411, 'left_w1': 0.5549175500175484, 'left_w2': 3.050704291907117, 'left_e0': 0.5161845351234418, 'left_e1': 1.1984224905354794, 'left_s0': -0.8904758473674826, 'left_s1': -0.38387869216832476}
right.move_to_joint_positions(neutral_right)
#!/usr/bin/env python
import sys
import time
import rospy
from std_msgs.msg import UInt32
from msg_types import *
import baxter_interface
from baxter_interface import Gripper, Head, Limb
rospy.init_node('run_condition')
voice_pub = rospy.Publisher('/voice', UInt32, queue_size=10)
time.sleep(1)
rightg = Gripper('right')
rightg.set_holding_force(50)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
neutral_right = {
'right_s0': 0.476301034638421,
'right_s1': -0.5606699779721187,
'right_w0': 0.07094661143970038,
'right_w1': 0.7037136864424336,
'right_w2': -0.28033498898605935,
'right_e0': -0.16566992509162468,
'right_e1': 1.3077186216723151
}
neutral_left = {
'left_w0': -0.15339807878854136,
