def __init__(self, armName, moveGroup, preemptionCheck):
# wait for linear path planner
rospy.wait_for_service('/apc/plan_linear_path')
rospy.wait_for_service('/apc/invert_trajectory')
rospy.wait_for_service('/apc/compute_ik_solution')
rospy.wait_for_service('/apc/plan_cartesian_linear_path')
self._linearPathPlanner = rospy.ServiceProxy('/apc/plan_linear_path', PlanLinearPath)
self._trajectoryInverter = rospy.ServiceProxy('/apc/invert_trajectory', InvertTrajectory)
self._inverseKinematicsSolver = rospy.ServiceProxy('/apc/compute_ik_solution', ComputeIKSolution)
self._cartesianLinearPathPlanner = rospy.ServiceProxy('/apc/plan_cartesian_linear_path',
PlanCartesianLinearPath)
self._armName = armName
self._moveGroup = moveGroup
self._roadmap = roadmap.Roadmap()
self._preemptionCheck = preemptionCheck
# We are using an RTree to save a mapping between poses and configurations in the roadmap
prop = index.Property()
prop.dimension = 6
self._poseIKCache = index.Index(properties=prop)
jointNames = ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']
if armName == 'left_arm':
self._armKinematics = baxter_pykdl.baxter_kinematics('left')
self._limb = baxter_interface.Limb('left')
self._jointNames = map(lambda x: 'left_' + x, jointNames)
else:
self._armKinematics = baxter_pykdl.baxter_kinematics('right')
self._limb = baxter_interface.Limb('right')
self._jointNames = map(lambda x: 'right_' + x, jointNames)
def __init__(self):
rospy.init_node('servo_node', anonymous=True)
self.listener = tf.TransformListener()
rospy.Subscriber("manipulation_state", std_msgs.msg.String,
self._get_command)
self.robot_pose = PoseWithCovariance() # where we think our robot is
## ROBOT CONTROL OBJECTS
self.limb_dict = {}
self.gripper_dict = {}
self.kin_dict = {}
self.limb_dict['right'] = baxter_interface.Limb('right')
self.kin_dict['right'] = baxter_kinematics('right')
self.gripper_dict['right'] = baxter_interface.Gripper('right')
self.gripper_dict['right'].calibrate()
self.limb_dict['left'] = baxter_interface.Limb('left')
self.kin_dict['left'] = baxter_kinematics('left')
#self.gripper_dict['left'] = baxter_interface.Gripper('left')
#self.gripper_dict['left'].calibrate()
## MOVE ARM TO START
limb = baxter_interface.Limb('right')
angles = limb.joint_angles()
angles['right_s0'] = -0.3148495567134812
angles['right_s1'] = 0.35051461003181705
angles['right_e0'] = 1.2755050251267215
angles['right_e1'] = 2.5648158773444116
angles['right_w0'] = -0.4306651061988299
angles['right_w1'] = -1.333213797491471
angles['right_w2'] = -1.67180584824316633
limb.move_to_joint_positions(angles, timeout=10.0)
pose = limb.endpoint_pose()
quat = pose['orientation']
self.t_desired_dict = {} # WHERE WE WANT THE ARM TO GO
self.r_desired_dict = {} # THE ORIENTATION OF THE ARM
self.t_desired_dict['right'] = np.array(
[[0.83728373, -0.39381423, 0.19123955]])
self.r_desired_dict['right'] = [quat.w, quat.x, quat.y, quat.z]
self.t_desired_dict['left'] = np.array(
[[0.83728373, -0.09381423, 0.19123955]])
self.r_desired_dict['left'] = np.array([[-0.0228398, -0.16423468, -0.98615684],\
[ 0.0376087, 0.9855748, -0.16500878], \
[ 0.9990315, -0.04085684, -0.01633368]])
## CONTROL VARIABLES
self.fridge_state = 0
# ENVIRONMENT INFO
self.fridge_handle_t = []
def main():
rospy.init_node('baxter_laser_reconstruction')
kinR = baxter_kinematics('right')
kinL = baxter_kinematics('left')
ic = image_converter()
start = time.time()
# analyze images for 10 seconds
detected2DPoints = []
reconstructed3D = []
while time.time() - start < 1:
if ic.newImage: # wait till we have a new image to do anything
detected2DPoints = findLine(ic.getImage())
# get plane parameters
testParams = getPlaneParams(kinR)
planeA = testParams[0][0]
planeB = testParams[0][1]
planeC= testParams[0][2]
planeD = np.dot(testParams[0].reshape(1,3),testParams[1])
# get projection matrix, camera is on left hand
Wleft = np.array([[406.00431,0,590.6386],[0,406.0743,420.9455],[0,0,1]])
P = getPFull(Wleft,kinL)
# Down sample for less points to plot
new2D = []
for i in range(len(detected2DPoints)):
if i%20 == 0:
print detected2DPoints[i]
new2D.append(detected2DPoints[i])
for detected2DPoint in new2D:
new3D = activeStereo3Dfrom2D(detected2DPoint,P,planeA,planeB,planeC,planeD)
reconstructed3D.append(new3D)
#print reconstructed3D
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
x = []
y = []
z = []
for point in reconstructed3D:
x.append(point[0])
y.append(point[1])
z.append(point[2])
plt.figure()
ax.scatter(x, y, z, c='r', marker='o')
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
def __init__(self, reconfig_server):
self.server = actionlib.SimpleActionServer('execute_verified_motion',
verified_motionAction,
self.execute, False)
self.feedback = None
self.result = None
self._dyn = reconfig_server
# control parameters
self._rate = 1000.0 # Hz
self._missed_cmds = 20.0 # Missed cycles before triggering timeout
# Include gripper(s)
#self._left_gripper = baxter_interface.gripper.Gripper("left")
# Limbs,limb parameters
self.limb = {}
self.limb["left_arm"] = baxter_interface.Limb('left')
self.limb["right_arm"] = baxter_interface.Limb('right')
self._left_arm = baxter_interface.Limb('left')
self._kin = {}
self._kin["left_arm"] = baxter_kinematics('left')
self._kin["right_arm"] = baxter_kinematics('right')
self._springs = dict()
self._damping = dict()
self._start_angles = dict()
self._goal_angles = dict()
self._spring_modifier = []
self._damping_modifier = []
self._sim_time = []
# Initialize Matlab
# Start matlab
print("\n Starting MATLAB...")
self.eng = matlab.engine.start_matlab() # start matlab
print("Matlab started.")
print("Initializing robotic toolbox files ...")
self.eng.baxter_matlab_robotics_toolbox_setup(nargout=0)
print("Initialized.")
# Is this necessary?
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
#self.move_to_neutral('left_arm')
#self.move_to_neutral('right_arm')
print('Starting server ...')
self.server.start()
def main():
joint_states = {
'right_initial': {
'right_e0': 2.498,
'right_e1': 2.158,
'right_s0': 0.826,
'right_s1': 0.366,
'right_w0': 2.809,
'right_w1': 1.867,
'right_w2': 0.411,
},
'left_initial': {
'left_e0': -1.738,
'left_e1': 1.828,
'left_s0': 0.247,
'left_s1': 0.257,
'left_w0': 0.0721,
'left_w1': -0.818,
'left_w2': 1.826,
},
}
rospy.init_node('demo', anonymous=True)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
arm_right = Limb("right")
kin_right = baxter_kinematics('right')
arm_left = Limb("left")
kin_left = baxter_kinematics('left')
set_joints(target_angles_right=joint_states['right_initial'])
# right arm movement
ik_move(hdr,
arm_right,
kin_right,
target_dx=0.35,
arm_position='right',
speedx=0.05)
ik_move(hdr,
arm_right,
kin_right,
target_dy=0.1,
arm_position='right',
speedy=0.1)
ik_move(hdr,
arm_right,
kin_right,
target_dx=-0.35,
arm_position='right',
speedx=0.1)
def limb_pose(limb_name): button = CuffOKButton(limb_name) rate = rospy.Rate(20) joint_pose = baxter_interface.Limb(limb_name).joint_angles() kinematics = baxter_kinematics(limb_name) endpoint_pose = kinematics.forward_position_kinematics(joint_pose) return endpoint_pose
def __init__(self):
self.limb_name = 'left'
self.other_limb_name = 'right'
self.limb = baxter_interface.Limb('left')
self.kinematics = baxter_kinematics('left')
self.jinv = self.kinematics.jacobian_pseudo_inverse()
self.Vx = np.array([0.01, 0, 0, 0, 0, 0])
def __init__(self):
rospy.logwarn("Initializing ")
self.rate_publisher = rospy.Publisher('robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._left_arm = baxter_interface.limb.Limb("left")
self._left_joint_names = self._left_arm.joint_names()
self._left_kin = baxter_kinematics('left')
rospy.sleep(2)
rospy.loginfo("Stuff")
# control parameters
self.pub_rate = 500.0 # Hz
rospy.loginfo("Getting robot state... ")
self.interface = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self.interface.state().enabled
rospy.loginfo("Enabling robot... ")
self.interface.enable()
self.lower_limits, self.upper_limits = hf.get_limits()
# set joint state publishing to 500Hz
self.rate_publisher.publish(self.pub_rate)
rospy.wait_for_service('check_collision')
self.input_received = False
def plan_validate(plan, dx, dy , dz, threshold = 0.2):
points = plan.joint_trajectory.points
if len(points) == 0: return False
score = max(sum((x1 - x0)**2 for x0, x1 in izip(point0.positions, point1.positions))
for point0, point1 in izip(points[:-1], points[1:]))
kin = baxter_kinematics('left')
rospy.loginfo('Plan scored %s for discontinuity' % score)
# points range in x, y, z direction
initial_pose = kin.forward_position_kinematics(points[0].positions)
maxx = initial_pose[0]
maxy = initial_pose[1]
maxz = initial_pose[2]
minx = maxx
miny = maxy
minz = maxz
for point in points:
pose = kin.forward_position_kinematics(point.positions)
maxx = max(maxx,pose[0])
maxy = max(maxy,pose[1])
maxz = max(maxz,pose[2])
minx = min(minx,pose[0])
miny = min(miny,pose[1])
minz = min(minz,pose[2])
print (maxx-minx), (maxy-miny), (maxz-minz)
if (maxx-minx) < dx and (maxy-miny) < dy and (maxz-minz) < dz:
withInRange = True
else:
withInRange = False
print dx, dy, dz
print withInRange
return False if score > threshold or not withInRange else True
def __init__(self, limb, reconfig_server):
self._dyn = reconfig_server
self._rKin = baxter_kinematics('right')
# control parameters
self._rate = 1000.0 # Hz
self._missed_cmds = 20.0 # Missed cycles before triggering timeout
# create our limb instance
self._limb = baxter_interface.Limb(limb)
# initialize parameters
self._springs = dict()
self._damping = dict()
self._start_angles = dict()
self._Torque = np.array([[0, 0, 0, 0, 0, 0, 0]])
self._output = np.array([0, 0, 0, 0, 0, 0])
# create cuff disable publisher
cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def test():
rospy.init_node('cut_test', anonymous=True)
joint_states = {
'observe':{
'left_e0': 0.06442719301757813,
'left_e1': 1.3721458131958009,
'left_s0': 0.44715539915771485,
'left_s1': -0.8076408838989259,
'left_w0': -0.061742726641845706,
'left_w1': 1.0239321747436525,
'left_w2': 1.4868108769592285,
}
}
set_joints(target_angles_left = joint_states['observe'])
rospy.sleep(1)
pub = rospy.Publisher('position_error', Point, queue_size=10)
arm = Limb('left')
kin = baxter_kinematics('left')
pose = get_current_pose(arm)
new_pose = update_current_pose(pose,0,0,-0.1)
start = rospy.get_time()
timeout = 5.0
while (rospy.get_time() - start) < timeout and not rospy.is_shutdown():
ik_pykdl(arm, kin, new_pose, side = 'left')
def test():
#sol = getSolutionAStar([0.55, -0.4, -0.6], {'right_s0': 0, 'right_s1': 0, 'right_e0': 0, 'right_e1': 0, 'right_w0': 0, 'right_w1': 0, 'right_w2': 0}, threshold = 0.1)
#print sol
rospy.init_node('baxter_kinematics')
from . import baxter
baxter.enable_robot(nodeEnabled=True)
sol = getSolution(
[0.5, 0, -0.2], {
'right_s0': 0,
'right_s1': 0,
'right_e0': 0,
'right_e1': 0,
'right_w0': 0,
'right_w1': 0,
'right_w2': 0
},
'right',
threshold=0.01)
print(sol)
rospy.init_node('baxter_kinematics')
kin = baxter_kinematics('right')
pos = kin.forward_position_kinematics(sol, var=True)
print(pos)
right = baxter_interface.Limb('right')
right.move_to_joint_positions(sol)
print(right.endpoint_pose()['position'])
print(right.joint_angles())
def __init__(self, limb, tf_listener):
self._limb = limb
self._tf_listener = tf_listener
# h: hand, c: camera
try:
self._tf_listener.waitForTransform(
'/' + self._limb + '_hand', '/' + self._limb + '_hand_camera',
rospy.Time(), rospy.Duration(4.0))
(self._t_hc, self._R_hc) = self._tf_listener.lookupTransform(
'/' + self._limb + '_hand', '/' + self._limb + '_hand_camera',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print 'Error! Cannot find [{}_hand_camera] to [{}_hand] tf.'.format(
self._limb, self._limb)
sys.exit(0)
self._R_hc = quaternion_matrix(self._R_hc)
# frame transforms from camera to hand, only look up once
self._T_hc = modern_robotics.RpToTrans(self._R_hc[:3, :3], self._t_hc)
self._Ad_hc = modern_robotics.Adjoint(self._T_hc)
# frame transforms from hand to body, look up in every loop
self._T_bh = np.zeros((4, 4))
self._Ad_bh = np.zeros((6, 6))
self._arm = baxter_interface.limb.Limb(self._limb)
self._kin = baxter_kinematics(self._limb)
def __init__(self):
rospy.logwarn("Initializing ")
self.rate_publisher = rospy.Publisher('robot/joint_state_publish_rate',
UInt16, queue_size=10)
self._left_arm = baxter_interface.limb.Limb("left")
self._left_joint_names = self._left_arm.joint_names()
self._left_kin = baxter_kinematics('left')
rospy.sleep(2)
rospy.loginfo("Stuff")
# control parameters
self.pub_rate = 500.0 # Hz
rospy.loginfo("Getting robot state... ")
self.interface = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self.interface.state().enabled
rospy.loginfo("Enabling robot... ")
self.interface.enable()
self.lower_limits, self.upper_limits = hf.get_limits()
# set joint state publishing to 500Hz
self.rate_publisher.publish(self.pub_rate)
rospy.wait_for_service('check_collision')
self.input_received = False
def main():
rospy.init_node('baxter_kinematics')
print '*** Baxter PyKDL Kinematics ***\n'
kin = baxter_kinematics('left')
# FK Position
print '\n*** Baxter Position FK ***\n'
kp = kin.forward_position_kinematics()
print kp
pos = [kp[0], kp[1], kp[2]]
rot = [kp[3], kp[4], kp[5], kp[6]]
limite = 0.0
kp2 = kin.forward_position_kinematics()
while kp2 != None:
limite = limite + 0.05
pos = [kp[0], kp[1], kp[2] + limite]
kp2 = kin.inverse_kinematics(pos, rot)
#pos2 = [kp[0]-limite,kp[1],kp[2]]
#kp = kin.inverse_kinematics(pos, rot)
#m = {'left_w0': l[4], 'left_w1': l[5], 'left_w2': l[6], 'left_e0': l[2], 'left_e1': l[3], 'left_s0': l[0], 'left_s1': l[1]}
#n = {'left_w0': l2[4], 'left_w1': l2[5], 'left_w2': l2[6], 'left_e0': l2[2], 'left_e1': l2[3], 'left_s0': l2[0], 'left_s1': l2[1]}
a = kp[1] + limite - 0.05
print a
print limite
def build_RRT():
rospy.init_node('build_RRT')
# Subscribes to waypoints from controller, sent in a set
z = rospy.Service('construct_RRT', construct_RRT, RRT_handler)
a = rospy.Service('construct_BIRRT', construct_BIRRT, BIRRT_handler)
global joint_limits
global limb
global kinematics
global joint_names
stepSize = 0.017 # ~1 degree in radians
# Left limb
joint_names = ['left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
limb = baxter_interface.Limb('left') #instantiate limb
kinematics = baxter_kinematics('left')
joint_limits = numpy.array([[-2.461, .890],[-2.147,1.047],[-3.028,3.028],[-.052,2.618],[-3.059,3.059],[-1.571,2.094],[-3.059,3.059]])
max_joint_speeds = numpy.array([2.0,2.0,2.0,2.0,4.0,4.0,4.0])
# Right limb
#joint_names = ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
#limb = baxter_interface.Limb('right')
#kinematics = baxter_kinematics('right')
#joint_limits = numpy.array([[-2.461, .890],[-2.147,1.047],[-3.028,3.028],[-.052,2.618],[-3.059,3.059],[-1.571,2.094],[-3.059,3.059]])
#max_joint_speeds = numpy.array([2.0,2.0,2.0,2.0,4.0,4.0,4.0])
global points
points = waypoints()
rospy.spin()
def __init__(self):
self.limb_name = 'left'
self.other_limb_name = 'right'
self.limb = baxter_interface.Limb('left')
self.kinematics = baxter_kinematics('left')
self.jinv = self.kinematics.jacobian_pseudo_inverse()
self.Vx = np.array([0.01, 0, 0, 0, 0, 0])
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 test():
rospy.init_node('cut_test', anonymous=True)
joint_states = {
'observe': {
'left_e0': 0.06442719301757813,
'left_e1': 1.3721458131958009,
'left_s0': 0.44715539915771485,
'left_s1': -0.8076408838989259,
'left_w0': -0.061742726641845706,
'left_w1': 1.0239321747436525,
'left_w2': 1.4868108769592285,
}
}
set_joints(target_angles_left=joint_states['observe'])
rospy.sleep(1)
pub = rospy.Publisher('position_error', Point, queue_size=10)
jacob = baxter_kinematics('left').jacobian_pseudo_inverse()
for i in range(3):
ik_move('left',
pub,
jacob,
control_mode='position',
target_dz=-0.2,
speed=1.0,
timeout=20)
ik_move('left',
pub,
jacob,
control_mode='position',
target_dz=0.2,
speed=1.0,
timeout=20)
def limb_pose(limb_name):
button = CuffOKButton(limb_name)
rate = rospy.Rate(20)
joint_pose = baxter_interface.Limb(limb_name).joint_angles()
kinematics = baxter_kinematics(limb_name)
endpoint_pose = kinematics.forward_position_kinematics(joint_pose)
return endpoint_pose
def sample(self):
""" Sample configurations within the workspace and store them. """
arm = raw_input(" Sample configurations for 'left' or 'right' arm: ")
if arm not in ['left', 'right']:
raise ValueError("Must be 'left' or 'right' arm!")
kin = baxter_kinematics(arm)
hull = Delaunay(self._data[:, :3])
n_configs = 300
configs = np.empty((n_configs, 7))
poses = np.empty((n_configs, 7))
idx = 0
lim = [q_lim(arm)[jn] for jn in joint_names(arm)]
while idx < n_configs:
try:
poses[idx, :], configs[idx, :] = \
sample_from_workspace(hull, kin, lim, arm)
idx += 1
except ValueError:
pass
print "\n"
path = raw_input(" Where to save poses2.txt and configurations2.txt: ")
if not os.path.exists(path):
os.makedirs(path)
np.savetxt(os.path.join(path, 'configurations2.txt'),
configs,
delimiter=',',
header='s0, s1, e0, e1, w0, w1, w2')
np.savetxt(os.path.join(path, 'poses2.txt'),
poses,
delimiter=',',
header='px, py, pz, ox, oy, oz, ow')
def __init__(self):
rospy.init_node('vision_server_left_v2')
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.newPosition = True
self.centerx = 230
self.centery = 350
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.found = 0
self.finish = 0
self.request = 0
self.close = 0
cv2.namedWindow('image')
self.np_image = np.zeros((400,640,3), np.uint8)
cv2.imshow('image',self.np_image)
s = rospy.Service('vision_server_left_v2', VisionVertical, self.handle_vision_req)
camera_topic = '/cameras/left_hand_camera/image'
self.right_camera = rospy.Subscriber(camera_topic, Image, self._on_camera)
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()
def main():
global limb_side
global kin
global rc
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
parser.add_argument(
'-l', '--limb', dest='limb', required=True, choices=['left', 'right'],
help='limb on which to attach joint springs'
)
parser.add_argument(
'-f', '--file', dest='file', required=False,
help='file from which to read trajectory to be played back with torque control'
)
args = parser.parse_args(rospy.myargv()[1:])
limb_side = args.limb
#filename = args.file
#print filename
print("Initializing node... ")
rospy.init_node("reactive_behavior_%s" % (limb_side,))
dynamic_cfg_srv = Server(JointSpringsExampleConfig,
lambda config, level: config)
#Initialize object from pyKDL library
kin = baxter_kinematics(args.limb)
rc = ReactiveControl(args.limb, dynamic_cfg_srv)
#print keys
#print lines
# register shutdown callback
rospy.on_shutdown(rc.clean_shutdown)
rc.move_to_neutral(0)
def __init__(self, arm, rate):
self.start_going = False
self.keep_going = True
self._path = None
self._limb = baxter_interface.Limb(arm)
self._kin = baxter_kinematics(arm)
self._rate = rate
rospy.Service('start_logging', TriggerLogging, self.start_logging)
def __init__(self, P=1/30000):
self.limb_name = 'left'
self.other_limb_name = 'right'
self.limb = baxter_interface.Limb(self.limb_name)
self.err_pub = rospy.Publisher('/error', Float64, queue_size=1)
self.P = P
self.kinematics = baxter_kinematics(self.limb_name)
self.jinv = self.kinematics.jacobian_pseudo_inverse()
def __init__(self, movegroup_interface, arm="right"):
self.arm = arm
# Initialize objects to call services to pause and unpause Gazebo physics.
self.pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
self.unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics",
Empty)
# Create objects to call services to set model configuration.
self.set_config_service = rospy.ServiceProxy(
"/gazebo/set_model_configuration", SetModelConfiguration)
# Create object to call services to start and stop controllers.
self.switch_controller_service = rospy.ServiceProxy(
"/robot/controller_manager/switch_controller", SwitchController)
# Simulation reset.
self.reset_world_service = rospy.ServiceProxy("/gazebo/reset_world",
Empty)
self.reset_sim_service = rospy.ServiceProxy("/gazebo/reset_simulation",
Empty)
# Baxter PyKDL objects.
self.baxter_right_kin_obj = baxter_kinematics('right')
self.baxter_left_kin_obj = baxter_kinematics('left')
# Order of joint names that joint limits are stored as.
# jn = ["right_s0", "right_s1", "right_e0", "right_e1", "right_w0", "right_w1", "right_w2"]
self.movegroup = movegroup_interface
# Create dictionaries of joint limits.
self.joint_limit_lower_dict = self.movegroup.recreate_dictionary(
"right", self.baxter_right_kin_obj.joint_limits_lower,
self.baxter_right_kin_obj._joint_names)
self.joint_limit_upper_dict = self.movegroup.recreate_dictionary(
"right", self.baxter_right_kin_obj.joint_limits_upper,
self.baxter_right_kin_obj._joint_names)
# Create sorted values.
self.lower_limits = np.array([
value
for (key, value) in sorted(self.joint_limit_lower_dict.items())
]) - 0.0005 * np.ones((7))
self.upper_limits = np.array([
value
for (key, value) in sorted(self.joint_limit_upper_dict.items())
]) + 0.0005 * np.ones((7))
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()
def __init__(self, filename, rate):
"""
Records joint data to a file at a specified rate.
"""
self._filename = filename
self._raw_rate = rate
self._rate = rospy.Rate(rate)
self._start_time = rospy.get_time()
self._done = False
self._limb_left = baxter_interface.Limb("left")
self._limb_right = baxter_interface.Limb("right")
# forward kinematics
self._limb_kin_left = baxter_pykdl.baxter_kinematics('left')
self._limb_kin_right = baxter_pykdl.baxter_kinematics('right')
self._arc_length_left = 0.0
self._arc_length_right = 0.0
self._gripper_left = baxter_interface.Gripper("left", CHECK_VERSION)
self._gripper_right = baxter_interface.Gripper("right", CHECK_VERSION)
self._io_left_lower = baxter_interface.DigitalIO('left_lower_button')
self._io_left_upper = baxter_interface.DigitalIO('left_upper_button')
self._io_right_lower = baxter_interface.DigitalIO('right_lower_button')
self._io_right_upper = baxter_interface.DigitalIO('right_upper_button')
self._io_left_lower.state_changed.connect(self._left_open_action)
self._io_left_upper.state_changed.connect(self._left_close_action)
# Use navigator for start and stop of recording
self._nav_left = baxter_interface.Navigator('left')
self._nav_left.button0_changed.connect(self._nav_b0_pressed)
self._is_started = False
self._pre_pos_left = None
# Verify Grippers Have No Errors and are Calibrated
if self._gripper_left.error():
self._gripper_left.reset()
if self._gripper_right.error():
self._gripper_right.reset()
if (not self._gripper_left.calibrated()
and self._gripper_left.type() != 'custom'):
self._gripper_left.calibrate()
if (not self._gripper_right.calibrated()
and self._gripper_right.type() != 'custom'):
self._gripper_right.calibrate()
def main():
rospy.init_node('baxter_velocity_control')
print '*** Baxter PyKDL Kinematics ***\n'
kin = baxter_kinematics('left')
print '\n*** Baxter Description ***\n'
kin.print_robot_description()
print '\n*** Baxter KDL Chain ***\n'
kin.print_kdl_chain()
# # FK Position
# print '\n*** Baxter Position FK ***\n'
# print kin.forward_position_kinematics()
# # FK Velocity
# # print '\n*** Baxter Velocity FK ***\n'
# # kin.forward_velocity_kinematics()
# # IK
# print '\n*** Baxter Position IK ***\n'
# pos = [0.582583, -0.180819, 0.216003]
# rot = [0.03085, 0.9945, 0.0561, 0.0829]
# print kin.inverse_kinematics(pos) # position, don't care orientation
# print '\n*** Baxter Pose IK ***\n'
# print kin.inverse_kinematics(pos, rot) # position & orientation
# # Jacobian
# print '\n*** Baxter Jacobian ***\n'
# print kin.jacobian()
# # Jacobian Transpose
# print '\n*** Baxter Jacobian Tranpose***\n'
# print kin.jacobian_transpose()
# # Jacobian Pseudo-Inverse (Moore-Penrose)
# print '\n*** Baxter Jacobian Pseudo-Inverse (Moore-Penrose)***\n'
# print kin.jacobian_pseudo_inverse()
# # Joint space mass matrix
# print '\n*** Baxter Joint Inertia ***\n'
# print kin.inertia()
# # Cartesian space mass matrix
# print '\n*** Baxter Cartesian Inertia ***\n'
# print kin.cart_inertia()
wobbler = Wobbler()
for i in range(300):
print i
J = kin.jacobian_pseudo_inverse()
A = np.matrix(J)
twist = np.array([[.0],[.0],[.0],[.0],[.0],[.0]])
q_dot = A*twist
q_dot_list = [i[0] for i in q_dot.tolist()]
print q_dot_list
wobbler.q_dot = q_dot_list
wobbler.wobble()
wobbler.q_dot = [.0,.0,.0,.0,.0,.0,.0]
wobbler.wobble()
rospy.on_shutdown(wobbler.clean_shutdown)
print("Done.")
def __init__(self, limb):
self.limb = limb #just a string
self.interface = baxter_interface.Limb(limb)
self.kinem = baxter_kinematics(limb)
self.jt_names = self.interface.joint_names()
self.curr_jts = self.get_curr_joints()
self.target_jts = self.curr_jts
self.target_jts_dict = self.interface.joint_angles()
self.target_pos = np.array([0., 0., 0.]) #just for initializing
def __init__(self, limb):
self.limb = limb #just a string
self.interface = baxter_interface.Limb(limb)
self.kinem = baxter_kinematics(limb)
self.jt_names = self.interface.joint_names()
self.curr_jts = self.get_curr_joints()
self.target_jts = self.curr_jts
self.target_jts_dict = self.interface.joint_angles()
self.target_pos = np.array([0., 0., 0.]) #just for initializing
def __init__(self, limb, rate):
self._neutral_config = NEUTRAL[limb]
self._kin = baxter_kinematics(limb)
self._limb = baxter_interface.Limb(limb)
self._nullspace_projector = TaskHeirarchy(7, rate)
rospy.logdebug(limb)
self._nullspace_projector.setup_cascaded_pose_vel(
np.zeros(7), self._kin.jacobian)
rospy.logdebug(self._nullspace_projector.get_full_jacobian())
def __init__(self, limb, reconfig_server):
self._dyn = reconfig_server
# control parameters
self._rate = 1000.0 # Hz
self._missed_cmds = 20.0 # Missed cycles before triggering timeout
self._z_cut_through = 0.020 # 0.0328
# parameters for calculating the accelaration
self._last_time = 0
self._last_v = 0
# score and time
self._score = 0
self._time_consumption = 0
# result
self.result = []
# create our limb instance
self._side = limb
self._limb = baxter_interface.Limb(limb)
self._kin = baxter_kinematics(limb)
self._jacob = self._kin.jacobian_pseudo_inverse()
self.joint_states = {
'observe':{
'left_e0': -0.662,
'left_e1': 1.65,
'left_s0': 0.0423,
'left_s1': -0.878,
'left_w0': 0.53,
'left_w1': 1.06,
'left_w2': -0.0706,
}
}
# initialize parameters
self._springs = dict()
self._damping = dict()
self._start_angles = dict()
# create cuff disable publisher
cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
self.pub = rospy.Publisher('position_error', Point, queue_size=10)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def _compare_ik_fk(self):
"""All the logged values should match in theory. In practice they're
fairly close.
"""
kinematics = baxter_kinematics(self.limb_name)
rospy.logerr(kinematics.forward_position_kinematics())
rospy.logerr(kinematics.forward_position_kinematics(
self.limb.joint_angles()))
rospy.logerr(self.limb.endpoint_pose())
def _compare_ik_fk(self):
"""All the logged values should match in theory. In practice they're
fairly close.
"""
kinematics = baxter_kinematics(self.limb_name)
rospy.logerr(kinematics.forward_position_kinematics())
rospy.logerr(
kinematics.forward_position_kinematics(self.limb.joint_angles()))
rospy.logerr(self.limb.endpoint_pose())
def main():
try:
movegroup = MoveGroupPythonInterface()
print("Created Movegroup.")
image_retriever = ImageRetriever()
print("Created Image Retriever.")
reset_manager = RobotResetManager(movegroup)
print("Created Robot Manager.")
kinematics = baxter_kinematics('right')
print("Created Kinematics Object.")
N = 1000
K = 5
dofs = 7
joint_states = np.zeros((N,dofs))
end_effector_state = np.zeros((N,dofs))
joint_state_seeds = np.zeros((N,dofs,K))
joint_names = ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
i=0
# Create N joint positions to test.
while i<N:
print("Currently Processing: ",i)
# Sample a joint state.
sampled_state = movegroup.right_arm.get_random_joint_values()
# Compute FK.
fk_result = movegroup.Compute_FK("right", movegroup.recreate_dictionary("right", sampled_state))
# If this state is valid.
if fk_result.error_code.val>0:
# Save states.
joint_states[i] = copy.deepcopy(sampled_state)
end_effector_state[i] = copy.deepcopy(movegroup.parse_pose(fk_result))
# Generate seeds.
joint_state_seeds[i] = np.random.random((dofs,K))-0.5
i = i+1
np.save("Joint_States.npy",joint_states)
np.save("EE_States.npy",end_effector_state)
np.save("Joint_State_Seeds.npy",joint_state_seeds)
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
embed()
def __init__(self, limb, reconfig_server):
self._dyn = reconfig_server
# control parameters
self._rate = 1000.0 # Hz
self._missed_cmds = 20.0 # Missed cycles before triggering timeout
self._z_cut_through = 0.020 # 0.0328
# parameters for calculating the accelaration
self._last_time = 0
self._last_v = 0
# score and time
self._score = 0
self._time_consumption = 0
# result
self.result = []
# create our limb instance
self._side = limb
self._limb = baxter_interface.Limb(limb)
self._kin = baxter_kinematics(limb)
self._jacob = self._kin.jacobian_pseudo_inverse()
self.joint_states = {
'observe': {
'left_e0': -0.662,
'left_e1': 1.65,
'left_s0': 0.0423,
'left_s1': -0.878,
'left_w0': 0.53,
'left_w1': 1.06,
'left_w2': -0.0706,
}
}
# initialize parameters
self._springs = dict()
self._damping = dict()
self._start_angles = dict()
# create cuff disable publisher
cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction'
self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1)
self.pub = rospy.Publisher('position_error', Point, queue_size=10)
# verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def compute_joint_velocities(self, cartesian_velocities, jinv):
joint_v = np.squeeze(np.asarray(self.jinv.dot(cartesian_velocities)))
jinv = baxter_kinematics(self.limb_name).jacobian_pseudo_inverse()
joint_v = np.squeeze(np.asarray(jinv.dot(cartesian_velocities)))
joint_v_dict = {'{}_{}'.format(self.limb_name, joint_name): val for
joint_name, val in zip(['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2'],
joint_v)}
joint_v_dict.update({'{}_{}'.format(self.other_limb_name, joint_name): 0.0 for
joint_name in ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']})
joint_v_dict.update({'head_nod':0.0 , 'head_pan':0.0, 'torso_t0':0.0})
return joint_v_dict
def getSolutionAStar(target,
startAngles,
threshold=0.01,
delta=0.01,
verbose=True):
kin = baxter_kinematics('right')
stateQ = []
visited = set()
curPosition = kin.forward_position_kinematics(startAngles)
if verbose:
print("Starting IK engine - starting pose:", curPosition)
curAngles = {
name: angleRanges[name].midpoint()
for name in list(startAngles.keys())
}
curPosition = kin.forward_position_kinematics(curAngles)
heappush(stateQ, (distance(curPosition, target), dict(curAngles)))
visited.add(Hashable(curAngles))
n = 0
movesUp = {name: 0 for name in list(curAngles.keys())}
movesDown = {name: 0 for name in list(curAngles.keys())}
while stateQ:
n += 1
d, curAngles = heappop(stateQ)
if d < threshold:
print("found solution in", n, "steps.")
return curAngles
if n > 10000:
if verbose:
print("Reached 10000 steps. Position:",
kin.forward_position_kinematics(curAngles))
print("Angles")
return None
deltaD = []
for name in list(curAngles.keys()):
#calculate distance from target if joint is rotated "up" or "down" by delta radians. Ignore results which exceed min/max joint angles
curAngles[name] += delta
if Hashable(curAngles) not in visited and curAngles[
name] in angleRanges[name]:
dUp = distance(kin.forward_position_kinematics(curAngles),
target)
visited.add(Hashable(curAngles))
heappush(stateQ, (dUp, dict(curAngles)))
curAngles[name] -= 2 * delta
if Hashable(curAngles) not in visited and curAngles[
name] in angleRanges[name]:
dDown = distance(kin.forward_position_kinematics(curAngles),
target)
visited.add(Hashable(curAngles))
heappush(stateQ, (dDown, dict(curAngles)))
curAngles[name] += delta
if verbose:
print("No useful moves found at step", n)
return None
def Init(self):
self._is_initialized= False
res= []
ra= lambda r: res.append(r)
self.default_face= 'config/baymax.jpg'
self.ChangeFace(self.default_face)
self.limbs= [None,None]
self.limbs[RIGHT]= baxter_interface.Limb(LRTostr(RIGHT))
self.limbs[LEFT]= baxter_interface.Limb(LRTostr(LEFT))
#self.joint_names= [[],[]]
#self.joint_names[RIGHT]= self.limbs[RIGHT].joint_names()
#self.joint_names[LEFT]= self.limbs[LEFT].joint_names()
self.kin= [None,None]
self.kin[RIGHT]= baxter_kinematics(LRTostr(RIGHT))
self.kin[LEFT]= baxter_kinematics(LRTostr(LEFT)) #tip_link=_gripper(default),_wrist,_hand
self.head= baxter_interface.Head()
ra(self.AddActC('r_traj', '/robot/limb/right/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction, time_out=3.0))
ra(self.AddActC('l_traj', '/robot/limb/left/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction, time_out=3.0))
self.epgripper= baxter_interface.Gripper('right', baxter_interface.CHECK_VERSION)
self.grippers= [self.epgripper, self.robotiq]
print 'Enabling the robot...'
baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION).enable()
print 'Calibrating electric parallel gripper...'
ra(self.epgripper.calibrate())
print 'Initializing and activating Robotiq gripper...'
ra(self.robotiq.Init())
if False not in res: self._is_initialized= True
return self._is_initialized
def __init__(self, name):
self._arm=name+'_arm'
self._limb=baxter_interface.limb.Limb(name)
self._robot=moveit_commander.RobotCommander()
self._group=moveit_commander.MoveGroupCommander(self._arm)
self._baxter_fk=baxter_kinematics(name)
rospy.Subscriber('/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1)
self._stop_reason = '' # 'collision' could cause a trajectory to be stopped
self._stop_lock = Lock()
def main():
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_vertical':{
'right_e0': 0.2784175126831055,
'right_e1': 1.8561167512207033,
'right_s0': -0.1464951650756836,
'right_s1': -0.5292233712158203,
'right_w0': -0.9572040105468751,
'right_w1': -1.200723460345459,
'right_w2': 0.004985437554931641,
},
}
roscpp_initialize(sys.argv)
rospy.init_node('demo', anonymous=True)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
arm = Limb("right")
kin = baxter_kinematics('right')
#set_joints(target_angles_right = joint_states['observe_midpoint'],target_angles_left = joint_states['observe_left'],timeout= 100000)
tracker=track()
rospy.on_shutdown(tracker.clean_shutdown)
#pour tomato
set_joints(target_angles_right = joint_states['observe_vertical'])
initial_pose = get_current_pose(hdr,arm)
#warm up the tracker after initial start up
tracker.track(initial_pose, hdr, arm, id = 1)
ik_move(hdr,arm, kin, target_dy = -0.12, speedy = 0.1)
tracker.rotate(math.pi/2)
ik_move(hdr,arm, kin, z = 0.47)
ik_move(hdr,arm, kin, target_dx = 0.3, speedx = 0.1)
#ik_move(hdr,arm, kin, target_dz = -0.08, speedz = 0.1)
ik_move(hdr,arm, kin, z = 0.35, speedz = 0.1)
ik_move(hdr,arm, kin, x = 0.75,speedx = 0.1)
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()
def baxterLimb_kin(baxter_limb="right"):
"""
This is baxter limb object
@baxter_limb = "right" or "left"
"""
try:
#rLimb=baxter_interface.Limb(baxter_limb)
rKin = baxter_kinematics(baxter_limb)
return rKin
except:
print "baxterLimb_kin error!"
return None
def __init__(self):
"""
'Wobbles' both arms by commanding joint velocities sinusoidally.
"""
self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate',
UInt16,
queue_size=10)
self._left_arm = baxter_interface.limb.Limb("left")
self._right_arm = baxter_interface.limb.Limb("right")
self._left_joint_names = self._left_arm.joint_names()
self._right_joint_names = self._right_arm.joint_names()
self._left_grip = baxter_interface.Gripper('left', CHECK_VERSION)
self._right_grip = baxter_interface.Gripper('right', CHECK_VERSION)
# calibrate
self._left_grip.calibrate()
self._right_grip.calibrate()
# self._left_grip_state = 'open'
# self._right_grip_state = 'open'
# control parameters
self._rate = 500.0 # Hz
self._max_speed = .05
self._arm = 'none'
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
# set joint state publishing to 500Hz
self._pub_rate.publish(self._rate)
rospy.Subscriber("joy", Joy, self._joystick_read)
# Dynamics
self.q_dot = [.0, .0, .0, .0, .0, .0, .0]
self.twist = np.array([[.0], [.0], [.0], [.0], [.0], [.0]])
print '*** Baxter PyKDL Kinematics ***\n'
self.kin = {}
self.kin['left'] = baxter_kinematics('left')
self.kin['right'] = baxter_kinematics('right')
self.pub_joy = rospy.Publisher('joy_commands',
joy_stick_commands,
queue_size=10)
def getSolution(target, startAngles, limb, threshold = 0.01, delta = 0.005, verbose = True):
kin = baxter_kinematics(limb)
curPosition = kin.forward_position_kinematics(startAngles)
if verbose:
print "Starting IK engine - starting pose:", curPosition
curAngles = {name: angleRanges[name].midpoint() for name in startAngles.keys()}
print curAngles
curPosition = kin.forward_position_kinematics(curAngles)
n = 0
movesUp = {name: 0 for name in curAngles.keys()}
movesDown = {name: 0 for name in curAngles.keys()}
while distance(curPosition, target) > threshold:
n += 1
if n > 10000:
if verbose:
print "Reached 10000 steps. Position:", curPosition
print "up:", movesUp
print "down", movesDown
return None
d = distance(curPosition, target)
deltaD = []
for name in curAngles.keys():
#calculate distance from target if joint is rotated "up" or "down" by delta radians. Ignore results which exceed min/max joint angles, or which are farther than current distance.
curAngles[name] += delta
if curAngles[name] in angleRanges[name]:
dUp = distance(kin.forward_position_kinematics(curAngles), target)
else:
dUp = float("inf")
curAngles[name] -= 2 * delta
if curAngles[name] in angleRanges[name]:
dDown = distance(kin.forward_position_kinematics(curAngles), target)
else:
dDown = float("inf")
curAngles[name] += delta
if dUp < d and dUp < dDown:
deltaD.append((dUp - d, name, True))
elif dDown < d:
deltaD.append((dDown - d, name, False))
if not deltaD:
if verbose:
print "No useful moves found at position:", curPosition
return None
deltaD.sort(key = lambda x: x[0])
if deltaD[0][2]:
curAngles[deltaD[0][1]] += delta
movesUp[deltaD[0][1]] += 1
else:
curAngles[deltaD[0][1]] -= delta
movesDown[deltaD[0][1]] += 1
curPosition = kin.forward_position_kinematics(curAngles)
print "found solution in", n, "steps."
return curAngles
def __init__(self,limb):
# Personal function to open right hand camera at half resolution. Replace with your own as needed.
# (The apriltags_ros package is too slow with full resolution).
#baxter.open_right_arm_cam_small()
transform=baxter.get_tf_listener()
transform.waitForTransform('/' + limb + '_hand','/' + limb + '_hand_camera',rospy.Time(0),rospy.Duration(5.0))
(self._cam2hand_t,self._cam2hand_R)=transform.lookupTransform('/' + limb + '_hand','/' + limb + '_hand_camera',rospy.Time(0))
self._cam2hand_t= np.concatenate((np.transpose(np.matrix(self._cam2hand_t)),np.matrix([[0]])),axis=0)
self._cam2hand_R=quaternion_matrix(self._cam2hand_R)
self._arm=baxter_interface.limb.Limb(limb)
self._kin = baxter_kinematics(limb)
def main():
joint_states = {
'right_initial':{
'right_e0': 2.498,
'right_e1': 2.158,
'right_s0': 0.826,
'right_s1': 0.366,
'right_w0': 2.809,
'right_w1': 1.867,
'right_w2': 0.411,
},
'left_initial':{
'left_e0': -1.738,
'left_e1': 1.828,
'left_s0': 0.247,
'left_s1': 0.257,
'left_w0': 0.0721,
'left_w1': -0.818,
'left_w2': 1.826,
},
}
rospy.init_node('demo', anonymous=True)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
arm_right = Limb("right")
kin_right = baxter_kinematics('right')
arm_left = Limb("left")
kin_left = baxter_kinematics('left')
set_joints(target_angles_right = joint_states['right_initial'])
# right arm movement
ik_move(hdr,arm_right, kin_right, target_dx = 0.35, arm_position = 'right', speedx = 0.05)
ik_move(hdr,arm_right, kin_right, target_dy = 0.1, arm_position = 'right', speedy = 0.1)
ik_move(hdr,arm_right, kin_right, target_dx = -0.35, arm_position = 'right', speedx = 0.1)
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()
def positioncontrol(TargetPosition):
kdl = baxter_pykdl.baxter_kinematics('right')
right_arm = baxter_interface.limb.Limb("right")
right_arm.set_joint_position_speed(0.8)
#right_arm.move_to_neutral()
#jointpositions = {'right_s0': 0, 'right_s1': 0, 'right_e0': 0, 'right_e1': 0, 'right_w0': 0, 'right_w1': 0, 'right_w2': 0}
#right_arm.move_to_joint_positions(jointpositions)
rate = rospy.Rate(10)
XT = np.array([TargetPosition[0], TargetPosition[1], (TargetPosition[2] + 0.05), -0.785745835105283, -0.1115831008789236, -3.1365214718905774])
XE = rightendpose()
GU = np.array([[-(XE[0]-XT[0])/abs(XE[2]-XT[2])],
[-(XE[1]-XT[1])/abs(XE[2]-XT[2])],
[-0.8],
[0],
[0],
[0]])
timeout = time.time() + 3
# on the tost 3 seconds X coffiecent 1 Y coffiecent 1
while True:
print('-------------')
#XE = rightendpose()
U = pow(XE[0]-XT[0],2)+pow(XE[1]-XT[1],2)+pow(XE[2]-XT[2],2)
joint_angles = right_arm.joint_angles()
JacobianInverse = kdl.jacobian_pseudo_inverse()
print('JacobianInverse', JacobianInverse)
delta = 0.01*np.dot(JacobianInverse, GU)
deltajointanglesdictionary = {'right_s0': delta[0], 'right_s1': delta[1], 'right_e0': delta[2], 'right_e1': delta[3], 'right_w0': delta[4], 'right_w1': delta[5], 'right_w2': delta[6]}
print('joint_angles',joint_angles)
print('adding',deltajointanglesdictionary)
#joint_angles = dict(Counter(joint_angles)+Counter(deltajointanglesdictionary))
#joint_angles = dict(joint_angles.items()+deltajointanglesdictionary.items())
joint_angles['right_s0'] += delta[0]
joint_angles['right_s1'] += delta[1]
joint_angles['right_e0'] += delta[2]
joint_angles['right_e1'] += delta[3]
joint_angles['right_w0'] += delta[4]
joint_angles['right_w1'] += delta[5]
joint_angles['right_w2'] += delta[6]
print('joint_angles',joint_angles)
right_arm.set_joint_positions(joint_angles,raw=False)
rate.sleep()
if time.time() > timeout:
break
def terminate(self):
server_sock.close()
kin = baxter_kinematics("right")
kinFK = kin.forward_position_kinematics()
if (float(kinFK[2]) <= 0.0):
cdn = [float(kinFK[0]), float(kinFK[1]), 0.05]
cmd = self.IKservice.solve(cdn)
if (cmd != 0):
self.right.move_to_joint_positions(cmd, 1)
self.open()
self.right.move_to_neutral()
time.sleep(7)
rospy.is_shutdown()
sys.exit()
def __init__(self):
"""
'Wobbles' both arms by commanding joint velocities sinusoidally.
"""
self._pub_rate = rospy.Publisher("robot/joint_state_publish_rate", UInt16, queue_size=10)
self._left_arm = baxter_interface.limb.Limb("left")
self._right_arm = baxter_interface.limb.Limb("right")
self._left_joint_names = self._left_arm.joint_names()
self._right_joint_names = self._right_arm.joint_names()
self._left_grip = baxter_interface.Gripper("left", CHECK_VERSION)
self._right_grip = baxter_interface.Gripper("right", CHECK_VERSION)
# calibrate
self._left_grip.calibrate()
self._right_grip.calibrate()
# self._left_grip_state = 'open'
# self._right_grip_state = 'open'
# control parameters
self._rate = 500.0 # Hz
self._max_speed = 0.05
self._arm = "none"
print ("Getting robot state... ")
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print ("Enabling robot... ")
self._rs.enable()
# set joint state publishing to 500Hz
self._pub_rate.publish(self._rate)
rospy.Subscriber("joy", Joy, self._joystick_read)
# Dynamics
self.q_dot = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.twist = np.array([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0]])
print "*** Baxter PyKDL Kinematics ***\n"
self.kin = {}
self.kin["left"] = baxter_kinematics("left")
self.kin["right"] = baxter_kinematics("right")
self.pub_joy = rospy.Publisher("joy_commands", joy_stick_commands, queue_size=10)
self.pub_save = rospy.Publisher("save_commands", UInt16, queue_size=10)
def robot_interface_njllrd():
rospy.init_node('robot_interface_njllrd')
rospy.sleep(5)
# Subscribes to waypoints from controller, sent in a set
z = rospy.Service('connect_waypoints', connect_waypoints, command_handler)
# Sends message to controller when it's done
rospy.Publisher('state', String, queue_size=10)
# If requested, returns endpoint pose
s = rospy.Service('request_endpoint', request_endpoint, handle_request_endpoint)
a = rospy.Service('request_orientation', request_orientation, handle_request_orientation)
trans = rospy.Service('request_translate', translate, handle_translate)
rot = rospy.Service('request_rotate', rotate, handle_rotate)
h = rospy.Service('request_home', home, handle_request_home)
h_cal = rospy.Service('request_home_calibrate', home_calibrate, handle_request_home_calibrate)
sw = rospy.Service('request_sweep', sweep_service, handle_sweep_start)
global joint_limits
global limb
global kinematics
global joint_names
global tol
tol = 0.01
arm = rospy.get_param("/arm_njllrd")
# Left limb
if arm == "left":
joint_names = ['left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2']
else:
joint_names = ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
limb = baxter_interface.Limb(arm) #instantiate limb
kinematics = baxter_kinematics(arm)
joint_limits = numpy.array([[-2.461, .890],[-2.147,1.047],[-3.028,3.028],[-.052,2.618],[-3.059,3.059],[-1.571,2.094],[-3.059,3.059]])
max_joint_speeds = numpy.array([2.0,2.0,2.0,2.0,4.0,4.0,4.0])
# Right limb
#joint_names = ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
#limb = baxter_interface.Limb('right')
#kinematics = baxter_kinematics('right')
#joint_limits = numpy.array([[-2.461, .890],[-2.147,1.047],[-3.028,3.028],[-.052,2.618],[-3.059,3.059],[-1.571,2.094],[-3.059,3.059]])
#max_joint_speeds = numpy.array([2.0,2.0,2.0,2.0,4.0,4.0,4.0])
global points
points = waypoints()
rospy.spin()
def __init__(self, limb_name='left'):
# initialize ros node
rospy.init_node('integrator')
rospy.loginfo("Initialized node Integrator")
# initialize baxter interface with limb
self.limb_name = limb_name
self.limb = baxter_interface.Limb(self.limb_name)
self.joint_names = self.limb.joint_names()
rospy.loginfo("Initialized limb interface")
# setup limb kinematics
self.limb_kinematics = baxter_pykdl.baxter_kinematics(self.limb_name)
# params
self.eps = 1e-9
# setup initial conditions to be current robot joint positions/velocities
rospy.loginfo("Getting Initial Conditions")
rospy.loginfo("Initial Joint Positions")
q_init = self.make_vec(self.limb.joint_angles)
print q_init
rospy.loginfo("Getting Joint Velocities")
q_dot_init = self.make_vec(self.limb.joint_velocities)
print q_dot_init
rospy.loginfo("Joint Momentums:")
p_init = np.dot(np.array(self.limb_kinematics.inertia()), q_dot_init)
print p_init
x_init = np.zeros(14)
x_init[:7] = q_init
x_init[7:] = p_init
print x_init
# time domain to evaluate on
T = np.linspace(0,4,10000)
# integrate
Q = scipy.integrate.odeint(self.f, x_init, T)
# plot
for i in xrange(7):
plt.plot(T,Q[:,i],label=self.joint_names[i])
plt.legend()
plt.figure()
for i in xrange(7):
plt.plot(T,Q[:,7+i],label="{0} momentum".format(self.joint_names[i]))
plt.legend()
plt.show()
def __init__(self, limb_name='left'):
rospy.init_node('joint_action_server')
baxter_interface.RobotEnable(CHECK_VERSION).enable()
# limb and joint parameters
self.limb_name = limb_name
self.limb = baxter_interface.Limb(limb_name)
self.limb_kin = baxter_kinematics(limb_name)
self.joint_names = self.limb.joint_names()
# time discretization paramters
self.dt = 0.008
self.deriv_step = 1e-5
self.secondary_objective = True
# secondary objective parameters
self.extra_motion_maximum = 0.00001
self.extra_motion_multiple = 1.1
# free-movement PID parameters
self.kp = 1.9
self.ki = 1.52
self.kd = 0.55
self.force_kp = 0.002
self.force_ki = 0.0005
self.force_kd = 0.0
# self.kp = 0.01
# self.ki = 0.01
# self.kd = 0.0
# self.dt = 0.012
# normal direction
self.surface_normal = np.array([0.0, 0.0, 1.0])
self.tangential_1 = np.array([1.0, 0.0, 0.0])
self.tangential_2 = np.array([0.0, 1.0, 0.0])
self.force_adjustments = True
self.desired_normal_force = -1.0
self.set_normal_vec = createService('set_normal_vec', SetNormalVec, self.set_normal_vec, "")
self.move_end_effector_trajectory = createService('move_end_effector_trajectory', JointAction, self.move_end_effector_trajectory, limb_name)
self.draw_on_plane_service = createService('draw_on_plane', JointAction, self.move_draw_on_plane, limb_name)
self.velocity_srv = createService('end_effector_velocity', EndEffectorVelocity, self.get_velocity_response, limb_name)
self.param_src = createService('set_parameters', SetParameters, self.parameter_response, limb_name)
self.position_srv = createService('end_effector_position', EndEffectorPosition, self.get_position_response, limb_name)
rospy.spin()
def limb_pose(limb_name):
"""Get limb pose at time of OK cuff button press."""
button = CuffOKButton(limb_name)
rate = rospy.Rate(20) # rate at which we check whether button was
# pressed or not
rospy.loginfo(
'Waiting for %s OK cuff button press to save pose' % limb_name)
# while not button.pressed and not rospy.is_shutdown():
# rate.sleep()
joint_pose = baxter_interface.Limb(limb_name).joint_angles()
# Now convert joint coordinates to end effector cartesian
# coordinates using forward kinematics.
kinematics = baxter_kinematics(limb_name)
endpoint_pose = kinematics.forward_position_kinematics(joint_pose)
return endpoint_pose
