本文整理匯總了Python中baxter_interface.Gripper.calibrate方法的典型用法代碼示例。如果您正苦於以下問題：Python Gripper.calibrate方法的具體用法？Python Gripper.calibrate怎麽用？Python Gripper.calibrate使用的例子？那麽, 這裏精選的方法代碼示例或許可以為您提供幫助。您也可以進一步了解該方法所在類baxter_interface.Gripper的用法示例。
在下文中一共展示了Gripper.calibrate方法的15個代碼示例，這些例子默認根據受歡迎程度排序。您可以為喜歡或者感覺有用的代碼點讚，您的評價將有助於係統推薦出更棒的Python代碼示例。
示例1: Abbe_Gripper
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
class Abbe_Gripper(object):
def __init__(self):
self._gripper = Gripper('left')
self._gripper.calibrate()
#right gripper isn't calibrated so it won't drop the RFID reader
#self._gripper_right = Gripper('right')
#self._gripper_right.calibrate()
def close(self,left=True):
if left:
self._gripper.close(block=True)
else:
self._gripper_right.close(block=True)
def open(self,left=True):
if left:
self._gripper.open(block=True)
else:
self._gripper_right.open(block=True)
def gripping(self,left=True):
if left:
return self._gripper.gripping()
else:
return self._gripper_right.gripping()示例2: Abbe_Gripper
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
class Abbe_Gripper(object):
def __init__(self):
self._gripper = Gripper('left')
self._gripper.calibrate()
#don't want it to drop rfid reader so disabling right gripperq
self._gripper_right = Gripper('right')
self._gripper_right.calibrate()
def close(self,left=True):
if left:
self._gripper.close(block=True)
else:
self._gripper_right.close(block=True)
def open(self,left=True):
if left:
self._gripper.open(block=True)
else:
self._gripper_right.open(block=True)
def gripping(self,left=True):
if left:
return self._gripper.gripping()
else:
return self._gripper_right.gripping()示例3: GripperConnect
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
class GripperConnect(object):
"""
Connects wrist button presses to gripper open/close commands.
Uses the DigitalIO Signal feature to make callbacks to connected
action functions when the button values change.
"""
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control {left, right}
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._close_io = DigitalIO('%s_upper_button' % (arm,)) # 'dash' btn
self._open_io = DigitalIO('%s_lower_button' % (arm,)) # 'circle' btn
self._light_io = DigitalIO('%s_lower_cuff' % (arm,)) # cuff squeeze
# outputs
self._gripper = Gripper('%s' % (arm,))
self._nav = Navigator('%s' % (arm,))
# connect callback fns to signals
if self._gripper.type() != 'custom':
self._gripper.calibrate()
self._open_io.state_changed.connect(self._open_action)
self._close_io.state_changed.connect(self._close_action)
else:
msg = (("%s (%s) not capable of gripper commands."
" Running cuff-light connection only.") %
(self._gripper.name.capitalize(), self._gripper.type()))
rospy.logwarn(msg)
if lights:
self._light_io.state_changed.connect(self._light_action)
rospy.loginfo("%s Cuff Control initialized...",
self._gripper.name.capitalize())
def _open_action(self, value):
if value:
rospy.logdebug("gripper open triggered")
self._gripper.open()
def _close_action(self, value):
if value:
rospy.logdebug("gripper close triggered")
self._gripper.close()
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
self._nav.inner_led = value
self._nav.outer_led = value示例4: track
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
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()示例5: main
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
def main():
factor=9
print("Right Init node")
rospy.init_node("Right_Ik_service_client", anonymous=True)
rate= rospy.Rate(1000)
right_gripper=Gripper('right')
right_gripper.calibrate()
right_gripper.open()
while not rospy.is_shutdown():
initial_position(round(6000*factor))#10ms
go_position(round(10000*factor),0.25)
for x in range (0,int(round(2000*factor))):
right_gripper.close()
#initial_position(10000)#3ms
go_box(round(250*factor))
for x in range (0,int(round(800*factor))):
right_gripper.open()
print "Objecto dejado"示例6: track
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
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示例7: track
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
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,
#.........這裏部分代碼省略.........
示例8: BaxterNode
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
class BaxterNode(object):
"""Helper class for creating Baxter nodes quickly, intended to be
sublclassed.
Attributes:
rs -- An instance of RobotEnable provided by baxter_interface.
ik -- An instance of IKHelper.
left_img -- Last recieved image from left camera, rectified and as an
OpenCV numpy array.
right_img -- Last recieved image from right camera, rectified and as an
OpenCV numpy array.
left_itb -- Last received state from left ITB (shoulder buttons).
left_itb -- Last received state from right ITB (shoulder buttons).
left_gripper -- Instance of Gripper from baxter_interface, for left
hand.
right_gripper -- Instance of Gripper from baxter_interface, for right
hand.
"""
############################################################################
def __init__(
self,
camera_averaging=False):
self._cvbr = CvBridge()
self.rs = RobotEnable()
self.ik = IKHelper()
camera_topic = '/cameras/{0}_camera/image_rect'
if camera_averaging:
camera_topic += '_avg'
self.left_img = None
self._left_camera_sub = rospy.Subscriber(
camera_topic.format('left_hand'),
Image,
self._on_left_imagemsg_received)
self.right_img = None
self._right_camera_sub = rospy.Subscriber(
camera_topic.format('right_hand'),
Image,
self._on_right_imagemsg_received)
self.head_img = None
self._head_camera_sub = rospy.Subscriber(
camera_topic.format('head'),
Image,
self._on_head_imagemsg_received)
self.left_itb = None
self._left_itb_sub = rospy.Subscriber(
'/robot/itb/left_itb/state',
ITBState,
self._on_left_itbmsg_received)
self.right_itb = None
self._right_itb_sub = rospy.Subscriber(
'/robot/itb/right_itb/state',
ITBState,
self._on_right_itbmsg_received)
self.left_gripper = Gripper('left')
self.right_gripper = Gripper('right')
self.head = Head()
self._display_pub = rospy.Publisher(
'/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
############################################################################
def start(self, spin=False, calibrate=False):
"""Start up the node and initalise Baxter.
Keyword arguments:
spin -- Enter a spin loop once initialised (default False).
calibrate -- Calibrate the grippers (default False).
"""
self.rs.enable()
# Wait for initial topic messages to come in.
while self.left_itb is None or \
self.right_itb is None:
rospy.sleep(100)
# Calibrate both grippers.
if calibrate:
self.left_gripper.calibrate()
self.right_gripper.calibrate()
if spin:
rospy.spin()
#.........這裏部分代碼省略.........
示例9: track
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
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()
rospy.sleep(2)
# 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)
self.joint_states = {
# 'observe':{
# 'right_e0': -0.365,
# 'right_e1': 1.061,
# 'right_s0': 0.920,
# 'right_s1': -0.539,
# 'right_w0': 0.350,
# 'right_w1': 1.105,
# 'right_w2': -0.221,
# },
'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_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_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,
},
}
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()示例10: BlockStackerNode
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
class BlockStackerNode(object):
RATE = 250
############################################################################
def __init__(self):
self._rs = RobotEnable()
self._cvbr = CvBridge()
self._sm = RobotStateMachine(self)
self.ik = IKHelper()
self.ik.set_right(0.5, 0.0, 0.0, wait=True)
self.left_img = None
self.right_img = None
self._left_camera_sub = rospy.Subscriber(
'/cameras/left_hand_camera/image_rect_avg',
Image,
self.on_left_imagemsg_received)
self._right_camera_sub = rospy.Subscriber(
'/cameras/right_hand_camera/image_rect_avg',
Image,
self.on_right_imagemsg_received)
self._display_pub = rospy.Publisher(
'/robot/xdisplay',
Image,
tcp_nodelay=True,
latch=True)
self.range = None
self._range_sub = rospy.Subscriber(
'/robot/range/right_hand_range/state',
Range,
self.on_rangemsg_received)
self.itb = None
self._itb_sub = rospy.Subscriber(
'/robot/itb/right_itb/state',
ITBState,
self.on_itbmsg_received)
self.gripper = Gripper('right')
self.gripper.calibrate()
self.gripper.close(block=True)
############################################################################
def start(self):
self._rs.enable()
self._sm.start()
rate = rospy.Rate(BlockStackerNode.RATE)
while not rospy.is_shutdown():
self._sm.run_step()
rate.sleep()
############################################################################
def on_left_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.left_img = img.copy()
self._sm.on_left_image_received(img)
def on_right_imagemsg_received(self, img_msg):
img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8')
img = cv2.resize(img, (640, 400))
self.right_img = img.copy()
self._sm.on_right_image_received(img)
def on_rangemsg_received(self, range_msg):
self.range = range_msg.range
def on_itbmsg_received(self, itb_msg):
self.itb = itb_msg
def display_image(self, img):
img = cv2.resize(img, (1024, 600))
print img.shape
if img.shape[2] == 1:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
img_msg = self._cvbr.cv2_to_imgmsg(img, 'bgr8')
self._display_pub.publish(img_msg)示例11: DmpLibrary
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
class DmpLibrary(object):
"""
The DMP Library class, currently supports the following DMPs
* Lateral grip
* Overhead grip
* return
Current non DMP actions that are supported
* pour
constructor:
required arg ->
dmp_record_file_dir : Path to the directory with dmp record files
optional arg ->
dmp_dims : The dimensions of the dmp to be trained (default: 7)
lateral_record_file : Name of the lateral record file (Default: lat_record_file)
vertical_record_file : Name of the vertical record file (Default: ver_record_file)
return_record_file : Name of the return record file (Default: return_record_file)
dummy : If set true, it will not execute but will only plan (Default: False)
"""
def __init__(self, dmp_record_file_dir, dmp_dims=7,
lateral_record_file=LATERAL_RECORD_FILE,
vertical_record_file=VERTICAL_RECORD_FILE,
return_record_file=RETURN_RECORD_FILE, dummy=False):
self.K = 100 # Spring constant, value taken from http://wiki.ros.org/dmp
self.D = 2.0 * np.sqrt(self.K) # Damping constant, value taken from http://wiki.ros.org/dmp
self.dt = 1.0 # Time resolution, Value taken from http://wiki.ros.org/dmp
self.num_bases = 4 # No of basis functions, value taken from http://wiki.ros.org/dmp
self.dmp_record_file_dir = dmp_record_file_dir
self.dmp_dims = dmp_dims
self.lateral_record_file = os.path.join(self.dmp_record_file_dir, lateral_record_file)
self.vertical_record_file = os.path.join(self.dmp_record_file_dir, vertical_record_file)
self.return_record_file = os.path.join(self.dmp_record_file_dir, return_record_file)
self.dummy = dummy
self.tf_listener = tf.TransformListener()
# the init velocity and goal threshold doesn't change across dmps, so just store them now
self.init_dot = []
for i in range(self.dmp_dims):
self.init_dot.append(0)
self.goal_thresh = []
for i in range(self.dmp_dims):
self.goal_thresh.append(0)
# Initialize move it group
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.group = moveit_commander.MoveGroupCommander("right_arm")
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory, queue_size=10)
self.integrate_iter = 5 # value taken from http://wiki.ros.org/dmp
self.seq_length = -1
self.right_gripper = Gripper('right')
# save the initial state
try:
self.init_pos, self.init_orient = self.tf_listener.lookupTransform('/base', '/right_gripper', rospy.Time(0)) #Plan to a different cup than demo
except:
time.sleep(1)
self.init_pos, self.init_orient = self.tf_listener.lookupTransform('/base', '/right_gripper', rospy.Time(0))
self.right_gripper.calibrate()
def getDMPplan(self, start_state, traj, goal):
"""
Function provided by the tutorial http://wiki.ros.org/dmp
"""
demotraj = DMPTraj()
for i in range(len(traj)):
pt = DMPPoint();
pt.positions = traj[i]
demotraj.points.append(pt)
demotraj.times.append(self.dt*i)
k_gains = [self.K]*self.dmp_dims
d_gains = [self.D]*self.dmp_dims
try:
# Learn from the trajectory (can probably move it to a dictionary)
rospy.wait_for_service('learn_dmp_from_demo')
learner_service = rospy.ServiceProxy('learn_dmp_from_demo', LearnDMPFromDemo)
learned_dmp = learner_service(demotraj, k_gains, d_gains, self.num_bases)
# Make the new dmp the current one
rospy.wait_for_service('set_active_dmp')
sad = rospy.ServiceProxy('set_active_dmp', SetActiveDMP)
sad(learned_dmp.dmp_list)
# Return a plan for the dmp
rospy.wait_for_service('get_dmp_plan')
gdp = rospy.ServiceProxy('get_dmp_plan', GetDMPPlan)
plan = gdp(start_state, self.init_dot, 0, goal, self.goal_thresh,
self.seq_length, 2* learned_dmp.tau, self.dt, self.integrate_iter)
except rospy.ServiceException, e:
print "There was an issue, with the service"%e
#.........這裏部分代碼省略.........
示例12: ArmCommander
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
class ArmCommander(Limb):
"""
This class overloads Limb from the Baxter Python SDK adding the support of trajectories via RobotState and RobotTrajectory messages
Allows to control the entire arm either in joint space, or in task space, or (later) with path planning, all with simulation
"""
def __init__(self, name, rate=100, fk='robot', ik='trac', default_kv_max=1., default_ka_max=0.5):
"""
:param name: 'left' or 'right'
:param rate: Rate of the control loop for execution of motions
:param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros"
:param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros"
:param default_kv_max: Default K maximum for velocity
:param default_ka_max: Default K maximum for acceleration
"""
Limb.__init__(self, name)
self._world = 'base'
self.kv_max = default_kv_max
self.ka_max = default_ka_max
self._gripper = Gripper(name)
self._rate = rospy.Rate(rate)
self._tf_listener = TransformListener()
self.recorder = Recorder(name)
# Kinematics services: names and services (if relevant)
self._kinematics_names = {'fk': {'ros': 'compute_fk'},
'ik': {'ros': 'compute_ik',
'robot': 'ExternalTools/{}/PositionKinematicsNode/IKService'.format(name),
'trac': 'trac_ik_{}'.format(name)}}
self._kinematics_services = {'fk': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['fk']['ros'], GetPositionFK),
'func': self._get_fk_ros},
'kdl': {'func': self._get_fk_pykdl},
'robot': {'func': self._get_fk_robot}},
'ik': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['ros'], GetPositionIK),
'func': self._get_ik_ros},
'robot': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['robot'], SolvePositionIK),
'func': self._get_ik_robot},
'trac': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['trac'], GetConstrainedPositionIK),
'func': self._get_ik_trac},
'kdl': {'func': self._get_ik_pykdl}}}
self._selected_ik = ik
self._selected_fk = fk
# Kinematics services: PyKDL
self._kinematics_pykdl = baxter_kinematics(name)
if self._selected_ik in self._kinematics_names['ik']:
rospy.wait_for_service(self._kinematics_names['ik'][self._selected_ik])
if self._selected_fk in self._kinematics_names['fk']:
rospy.wait_for_service(self._kinematics_names['fk'][self._selected_fk])
# Execution attributes
rospy.Subscriber('/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1)
rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name), DigitalIOState, self._cb_dig_io, queue_size=1)
self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped
self._stop_lock = Lock()
action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(self.name)
self.client = SimpleActionClient(action_server_name, FollowJointTrajectoryAction)
self._display_traj = rospy.Publisher("/move_group/display_planned_path", DisplayTrajectory, queue_size=1)
self._gripper.calibrate()
self.client.wait_for_server()
######################################### CALLBACKS #########################################
def _cb_collision(self, msg):
if msg.collision_state:
with self._stop_lock:
self._stop_reason = 'collision'
def _cb_dig_io(self, msg):
if msg.state > 0:
with self._stop_lock:
self._stop_reason = 'cuff'
#############################################################################################
def endpoint_pose(self):
"""
Returns the pose of the end effector
:return: [[x, y, z], [x, y, z, w]]
"""
pose = Limb.endpoint_pose(self)
return [[pose['position'].x, pose['position'].y, pose['position'].z],
[pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w]]
def endpoint_name(self):
return self.name+'_gripper'
def group_name(self):
return self.name+'_arm'
def joint_limits(self):
xml_urdf = rospy.get_param('robot_description')
dict_urdf = xmltodict.parse(xml_urdf)
joints_urdf = []
joints_urdf.append([j['@name'] for j in dict_urdf['robot']['joint'] if j['@name'] in self.joint_names()])
joints_urdf.append([[float(j['limit']['@lower']), float(j['limit']['@upper'])] for j in dict_urdf['robot']['joint'] if j['@name'] in self.joint_names()])
# reorder the joints limits
return dict(zip(self.joint_names(),
[joints_urdf[1][joints_urdf[0].index(name)] for name in self.joint_names()]))
#.........這裏部分代碼省略.........
示例13: Gripper
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
#!/usr/bin/env python
import rospy
from baxter_interface import Gripper
if __name__ == '__main__':
rospy.init_node('gripperTools', anonymous=True)
gripper = Gripper('left')
gripper.calibrate()
示例14: __init__
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
def __init__(self):
pygame.init()
rospy.init_node('node_mouse', anonymous=True)
right_gripper=Gripper('right')
left_gripper=Gripper('left')
#limb_0=baxter_interface.Limb('right')
#limb_0.set_joint_positions({'right_w2': 0.00})
def set_w2():
limb=baxter_interface.Limb('left')
current_position = limb.joint_angle('left_w2')
#send=current_position+delta
#if(send>2.80 or send<-2.80):
# delta=-delta
# time.sleep(0.15)
#print send
return current_position
#joint_command = {joint_name: send}
#limb.set_joint_positions(joint_command)
#if(current_position-send>0):
# delta=-1
def set_s1():
limb=baxter_interface.Limb('left')
current_position = limb.joint_angle('left_s1')
#send=current_position+delta
#if(send>2.80 or send<-2.80):
# delta=-delta
# time.sleep(0.15)
#print send
return current_position
def set_e1():
limb=baxter_interface.Limb('left')
current_position = limb.joint_angle('left_e1')
#send=current_position+delta
#if(send>2.80 or send<-2.80):
# delta=-delta
# time.sleep(0.15)
#print send
return current_position
pygame.display.set_caption('Game')
pygame.mouse.set_visible(True)
vec = Vector3(1, 2, 3)
right_gripper.calibrate()
left_gripper.calibrate()
try:
pub = rospy.Publisher('mouse', Vector3, queue_size=1)
rate = rospy.Rate(1000) # 10hz
screen = pygame.display.set_mode((640,480), 0, 32)
pygame.mixer.init()
a=0 ,0 ,0
while not rospy.is_shutdown():
for event in pygame.event.get():
if event.type == pygame.MOUSEBUTTONDOWN or event.type == pygame.MOUSEBUTTONUP:
a = pygame.mouse.get_pressed()
print a
weq=0
if a[0]==1:
left_gripper.open()
else:
left_gripper.close()
if a[2]==1:
right_gripper.open()
else:
right_gripper.close()
vec.x=set_w2()
vec.y=a[1]
vec.z=0
print vec
pub.publish(vec)
pygame.mixer.quit()
pygame.quit()
except rospy.ROSInterruptException:
pass示例15: Gripper
# 需要導入模塊: from baxter_interface import Gripper [as 別名]
# 或者: from baxter_interface.Gripper import calibrate [as 別名]
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)
# 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)
def wait():
raw_input("Press Enter to continue...")
def send_voice(msg):
print "Baxter: %s" % index_to_string[msg]
voice_pub.publish(data = msg)