本文整理汇总了Python中baxter_interface.Limb类的典型用法代码示例。如果您正苦于以下问题：Python Limb类的具体用法？Python Limb怎么用？Python Limb使用的例子？那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Limb类的15个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: __init__
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()示例2: __init__
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber('/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher('/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None示例3: set_joints
def set_joints( target_angles_right, target_angles_left):
right = Limb("right")
left = Limb("left")
reach_right = False
reach_left = False
while not reach_right or not reach_left:
right.set_joint_positions(target_angles_right)
left.set_joint_positions(target_angles_left)
current_angles_right = right.joint_angles()
current_angles_left = left.joint_angles()
for k, v in current_angles_right.iteritems():
if abs(target_angles_right[k] - v) > 0.01:
reach_right = False
break
reach_right = True
for k, v in current_angles_left.iteritems():
if abs(target_angles_left[k] - v) > 0.01:
reach_left = False
break
reach_left = True示例4: __init__
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()示例5: record
def record():
rospy.init_node('record')
RobotEnable(CHECK_VERSION).enable()
baxter_limb = Limb(limb_name)
while True:
initial_time = rospy.get_time()
forces_vec3 = baxter_limb.endpoint_effort()['force']
forces = np.array([forces_vec3.x, forces_vec3.y, forces_vec3.z])
print "===========\nEndpoint Forces: \nF_x: {x} \nF_y: {y} \nF_z: {z}\n===========\n".format(forces_vec3)
after_time = rospy.get_time()
rospy.sleep(dt - (after_time - initial_time))示例6: __init__
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)示例7: endpoint_pose
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]]示例8: __init__
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.3)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.3)
self._left_iksvc = rospy.ServiceProxy(
_IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
_IKSVC_RIGHT_URI,
SolvePositionIK)
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate',
UInt16)
self._joint_update_pub.publish(250)示例9: track
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()示例10: __init__
def __init__(self):
rospy.init_node('vision_server_right')
self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self.busy = False
self.dx = 0
self.dy = 0
self.avg_dx = -1
self.avg_dy = -1
self.H = homography()
self.framenumber = 0
self.history_x = np.arange(0,10)*-1
self.history_y = np.arange(0,10)*-1
self.bowlcamera = None
self.newPosition = True
self.foundBowl = 0
self.centerx = 400
#self.centerx = 250
self.centery = 150
#self.centery = 190
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.v_joint = np.arange(7)
self.v_end = np.arange(6)
self.cmd = {}
self.found = False
self.finish = 0
self.distance = 10
self.gripper = Gripper("right")
#self.gripper.calibrate()
cv2.namedWindow('image')
self.np_image = np.zeros((400,640,3), np.uint8)
cv2.imshow('image',self.np_image)
#self._set_threshold()
s = rospy.Service('vision_server_vertical', Vision, self.handle_vision_req)
camera_topic = '/cameras/right_hand_camera/image'
self.right_camera = rospy.Subscriber(camera_topic, Image, self._on_camera)
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
print "\nReady to use right hand vision server\n"
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()示例11: Baxter_Deli
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示例12: set_joints
def set_joints( target_angles_right = None, target_angles_left = None,timeout= 40000):
right = Limb("right")
left = Limb("left")
if target_angles_right == None:
reach_right = True
else:
reach_right = False
if target_angles_left == None:
reach_left = True
else:
reach_left = False
time = 0
while not reach_right or not reach_left:
if target_angles_right: right.set_joint_positions(target_angles_right)
if target_angles_left: left.set_joint_positions(target_angles_left)
current_angles_right = right.joint_angles()
current_angles_left = left.joint_angles()
if reach_right == False:
for k, v in current_angles_right.iteritems():
if abs(target_angles_right[k] - v) > 0.01:
reach_right = False
break
reach_right = True
if reach_left == False:
for k, v in current_angles_left.iteritems():
if abs(target_angles_left[k] - v) > 0.01:
reach_left = False
break
reach_left = True
time+=1
if time > timeout:
print "Time out"
break示例13: __init__
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,
#.........这里部分代码省略.........
示例14: __init__
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.6)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.6)示例15: IK
class IK(object):
def __init__(self):
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.6)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.6)
def neutral(self):
self._left_arm.move_to_neutral()
self._right_arm.move_to_neutral()
def ik_calculate(self,limb,pos,rot):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
#rotation -0.5 is perp and -1.0 is parallel for rot[2]
quat = transformations.quaternion_from_euler(rot[0], rot[1], rot[2])
pose = PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=pos[0], #depth
y=pos[1], #lateral
z=pos[2], #height
),
orientation=Quaternion(
quat[0],
quat[1],
quat[2],
quat[3],
),
),
)
ikreq.pose_stamp.append(pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e,))
return 1
# Check if result valid, and type of seed ultimately used to get solution
# convert rospy's string representation of uint8[]'s to int's
resp_seeds = struct.unpack('<%dB' % len(resp.result_type),
resp.result_type)
if (resp_seeds[0] != resp.RESULT_INVALID):
seed_str = {
ikreq.SEED_USER: 'User Provided Seed',
ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp_seeds[0], 'None')
#print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" %
# (seed_str,))
# Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
return limb_joints
else:
pass
#print pose
#print("INVALID POSE - No Valid Joint Solution Found.")
return 0