def __init__(self):
self.larm = baxter_interface.Limb("left")
self.rarm = baxter_interface.Limb("right")
self.larm.set_command_timeout(1)
self.rarm.set_command_timeout(1)
def main():
right_filename = 'right_joint_state'
leftt_filename = 'left_joint_state'
rf = open(right_filename, 'w')
lf = open(leftt_filename, 'w')
epilog = ''
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__,
epilog=epilog)
parser.parse_args(rospy.myargv()[1:])
print("Initializing node... ")
rospy.init_node("rsdk_joint_position_keyboard")
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
if (init_state == False):
print ("Baxter must be enabled")
return
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
done = False
print_mode = False
i = 0;
start_force = 0;
max_force = 0;
while not done and not rospy.is_shutdown():
c = getch()
if c:
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c == 't' or c == 'T':
print left.joint_efforts()
print right.joint_efforts()
elif c == 'a' or c == 'A':
print left.joint_angles()
print right.joint_angles()
elif c == 'v' or c == 'V':
print left.joint_velocities()
print right.joint_velocities()
elif c == 'e' or c == 'E':
print left.endpoint_pose()
#print right.endpoint_pose()
elif c == 'f' or c == 'F':
print left.endpoint_effort()
print right.endpoint_effort()
elif c == 'y' or c == 'Y':
print left.endpoint_velocity()
print right.endpoint_velocity()
def send_image(path):
img = cv2.imread(path)
msg = cv_bridge.CvBridge().cv2_to_imgmsg(img, encoding="bgr8")
pub = rospy.Publisher('/robot/xdisplay', Image, queue_size=100, latch=True)
pub.publish(msg)
# Sleep to allow for image to be published.
rospy.sleep(1)
if __name__ == '__main__':
# initialize the ROS node, enable robot
rospy.init_node("Baxter_Move_Subscribers")
rs = baxter_interface.RobotEnable()
rs.enable()
# create an instance of baxter_interface's Limb, Gripper and head classes
right_limb = baxter_interface.Limb("right")
left_limb = baxter_interface.Limb("left")
right_gripper = baxter_interface.Gripper('right')
left_gripper = baxter_interface.Gripper('left')
head = baxter_interface.Head()
# set limb and gripper parameters
right_limb.set_joint_position_speed(0.6) #0-1.0
# right_gripper.set_velocity(100.0) #0-100.0
# right_gripper.calibrate()
# left_limb.set_joint_position_speed(0.6) #0-1.0
left_gripper.set_velocity(100.0) #0-100.0
left_gripper.calibrate()
# initialize current and goal pose of the robot
def main():
rospy.init_node("dmp_ability_test", anonymous=True)
# this file is recoded by baxter
# run this command to record data "rosrun baxter_examples joint_recorder.py -f path/file_name
global data_path, train_len, tau, limb_name
limb = baxter_interface.Limb(limb_name)
train_set = pd.read_csv(data_path) #using pandas read data
resample_t = np.linspace(train_set.values[0, 0], train_set.values[-1, 0],
train_len) # resampling the time
postion_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 1])
postion_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 2])
postion_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 3])
orientation_x = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 4])
orientation_y = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 5])
orientation_z = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 6])
orientation_w = np.interp(resample_t, train_set.values[:, 0],
train_set.values[:, 7])
traj = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] * train_len
for i in range(train_len):
traj[i] = [
postion_x[i], postion_y[i], postion_z[i], orientation_x[i],
orientation_y[i], orientation_z[i], orientation_w[i]
]
train_set_T = np.array([
np.array([
postion_x, postion_y, postion_z, orientation_x, orientation_y,
orientation_z, orientation_w
]).T
])
param_w, base_function = train(train_set_T)
# object_vision = get_base_camera_pose(flag = True)
end_pose = limb.endpoint_pose()
# get current angles
endpoint_pose = [
end_pose['position'].x, end_pose['position'].y, end_pose['position'].z,
end_pose['orientation'].x, end_pose['orientation'].y,
end_pose['orientation'].z, end_pose['orientation'].w
]
# ipdb.set_trace()
start_point = traj[0]
# start_point = endpoint_pose
# print start_point
# ending_point = object_vision
ending_point = traj[-1]
# print ending_point
dmp = pydmps.dmp_discrete.DMPs_discrete(n_dmps=7, n_bfs=500, w=param_w)
for i in range(7):
dmp.y0[i] = start_point[i] #set the initial state
dmp.goal[i] = ending_point[i] # set the ending goal
y_track, dy_track, ddy_track = dmp.rollout(tau=tau) # ?
time_dmp = np.linspace(0, train_set.values[-1, 0], train_len)
dmp_data_w_time = np.column_stack((time_dmp, y_track))
# ipdb.set_trace()
####################################### For plotting
#creat fig
fig = plt.figure(1)
ax = Axes3D(fig)
plt.xlabel('X')
plt.ylabel('Y')
#plot traj fig
ax.plot(postion_x, postion_y, postion_z, linewidth=2, alpha=0.3)
#Plot plan fig
ax.plot(y_track[:, 0],
y_track[:, 1],
y_track[:, 2],
linewidth=2,
alpha=0.3)
#Plot plan fig
#show the plot
plt.draw()
plt.show() # uncomment to plot
return dmp_data_w_time, limb_name
def __init__(self, arm, distance, colours, board):
# piece data
self.playerPiece = 'X'
self.playerPieceIndex = 1 # ( 1 or 2, 3 = board)
self.robotPiece = 'R'
self.robotPieceIndex = 2
# space detection
self.board = board
self.boardHeight = 0.2971
self.boardWidth = 0.410
self.storePointPix = True
self.detectedAllCorners = False
self.pixelPoints = []
self.board_tolerance = 0.00375
# board xyz
spaces = self.board.getSpaces()
self.boardZ = len(spaces) # levels
self.boardY = len(spaces[0]) # rows
self.boardX = len(spaces[0][0]) # columns
# Background subtraction
self.bgsubImgmsg = None
self.imgmsg = None
# Circle piece detection
self.numCircleFrames = 1
self.spacePixelDist = 50.0
# arm ("left" or "right")
self.limb = arm
self.limb_interface = baxter_interface.Limb(self.limb)
if arm == "left":
self.other_limb = "right"
else:
self.other_limb = "left"
self.other_limb_interface = baxter_interface.Limb(self.other_limb)
# gripper ("left" or "right")
self.gripper = baxter_interface.Gripper(arm)
# object color dictionary data
self.objects = colours
# zeroed lists for pixel coordinates of objects
self.x = np.zeros(shape=(len(colours)), dtype=int)
self.y = np.zeros(shape=(len(colours)), dtype=int)
# speeds
self.normal = 0.8
self.slow = 0.1
# start positions
self.start_pos_x = 0.50 # x = front back
self.start_pos_y = 0.30 # y = left right
self.start_pos_z = 0.15 # z = up down
self.roll = -1.0 * math.pi # roll = horizontal
self.pitch = -0.0 * math.pi # pitch = vertical
self.yaw = 0.0 * math.pi # yaw = rotation
self.pose = [self.start_pos_x, self.start_pos_y, self.start_pos_z, \
self.roll, self.pitch, self.yaw]
# distances
self.distance = distance
#self.pixelHeightConst = 0.0022762 # constant for distance = 0.383, change for different heights!!!
self.block_height = 0.04
self.block_pickup_height = self.distance - self.block_height
self.block_grip_height = 0.102
self.block_tolerance = 0.005
# camera parameters (NB. other parameters in open_camera)
# self.cam_calib = 0.0025 # original # meters per pixel at 1 meter
self.cam_calib = 0.0022762 # constant for distance = 0.383, change for different heights!!!
self.cam_x_offset = 0.045 # original camera gripper offset
self.cam_y_offset = -0.015 #-0.01
#self.cam_x_offset = 0.045 # camera gripper offset
#self.cam_y_offset = -0.02
self.width = 960 # Camera resolution
self.height = 600
# Enable the actuators
baxter_interface.RobotEnable().enable()
# set speed as a ratio of maximum speed
self.limb_interface.set_joint_position_speed(0.8)
self.other_limb_interface.set_joint_position_speed(0.8)
# calibrate the gripper
self.gripper.calibrate()
# set camera resolution
self.config_camera(self.limb, self.width, self.height)
# subscribe to required camera
self.subscribe_to_camera(self.limb)
# Display camera feed to face display
self.displayCamera = False
self.displayContours = False
self.pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True,
queue_size=1)
# move other arm out of harms way
if arm == "left":
self.baxter_ik_move("right", (0.25, -0.50, 0.2, math.pi, 0.0, 0.0))
else:
self.baxter_ik_move("left", (0.25, 0.50, 0.2, math.pi, 0.0, 0.0))
def __init__(self):
if not self.load_parameters(): sys.exit(1)
self._limb = baxter_interface.Limb(self._arm)
self._kin = baxter_kinematics(self._arm)
self._planner = PathPlanner('{}_arm'.format(self._arm))
rospy.on_shutdown(self.shutdown)
plan = self._planner.plan_to_joint_pos(
np.array([-0.6, -0.4, -0.5, 0.6, -0.4, 1.1, -0.5]))
self._planner.execute_plan(plan)
rospy.sleep(5)
################################## Tensorflow bullshit
if self._learning_bool:
# I DON'T KNOW HOW THESE WORK
#define placeholders
observation_space = spaces.Box(low=-100,
high=100,
shape=(21, ),
dtype=np.float32)
action_space = spaces.Box(low=-50,
high=50,
shape=(56, ),
dtype=np.float32)
self._x_ph, self._u_ph = core.placeholders_from_spaces(
observation_space, action_space)
#define actor critic
#TODO add in central way to accept arguments
self._pi, self._logp, self._logp_pi, self._v = core.mlp_actor_critic(
self._x_ph,
self._u_ph,
hidden_sizes=(128, 2),
action_space=action_space)
# POLY_ORDER = 2
# self._pi, self._logp, self._logp_pi, self._v = core.polynomial_actor_critic(
# self._x_ph, self._u_ph, POLY_ORDER, action_space=action_space)
#start up tensorflow graph
var_counts = tuple(core.count_vars(scope) for scope in [u'pi'])
print(u'\nYoyoyoyyoyo Number of parameters: \t pi: %d\n' %
var_counts)
self._tf_vars = [
v for v in tf.trainable_variables() if u'pi' in v.name
]
self._num_tf_vars = sum(
[np.prod(v.shape.as_list()) for v in self._tf_vars])
print("tf vars is of length: ", len(self._tf_vars))
print("total trainable vars: ", len(tf.trainable_variables()))
self._sess = tf.Session()
self._sess.run(tf.global_variables_initializer())
print("total trainable vars: ", len(tf.trainable_variables()))
self._last_time = rospy.Time.now().to_sec()
current_position = utils.get_joint_positions(self._limb).reshape(
(7, 1))
current_velocity = utils.get_joint_velocities(self._limb).reshape(
(7, 1))
self._last_x = np.vstack([current_position, current_velocity])
self._last_xbar = self._last_x
if self._learning_bool:
self._last_a = self._sess.run(self._pi,
feed_dict={
self._x_ph:
self.preprocess_state(
self._last_x).reshape(1, -1)
})
else:
self._last_a = np.zeros((1, 56))
#################################### Set Tensorflow from saved params
self._READ_PARAMS = self._is_test_set
PARAM_INDEX = 201 ## This is the index of the learned parameters that you'll use (which epoch)
if self._learning_bool:
if self._READ_PARAMS:
import dill
# Put ABSOLUTE path to location of learned_params.pkl here,
# then remove the NotImplementedError. (This will probably be the
# same as the PREFIX variable in data_collector.py)
# eg. PREFIX = "/home/cc/ee106a/fa19/staff/ee106a-taf/Desktop/data"
PREFIX = "/home/cc/ee106b/sp20/staff/ee106b-laa/Desktop/data/read_params"
# raise NotImplementedError
lp = dill.load(open(PREFIX + "/learned_params.pkl", "rb"))
print("Running training set using data from epoch number: " +
str(PARAM_INDEX))
param_list = []
for param in lp[PARAM_INDEX][1]:
p_msg = Parameters(param)
param_list.append(p_msg)
lp_msg = LearnedParameters(param_list)
self._sess.run(
tf.assign(
tf.get_default_graph().get_tensor_by_name(
'pi/log_std:0'), tf.zeros((56, ))))
self.params_callback(lp_msg)
self._last_a = self._sess.run(
self._pi,
feed_dict={
self._x_ph:
self.preprocess_state(self._last_x).reshape(1, -1)
})
##################################### Controller params
self._A = np.vstack([
np.hstack([np.zeros((7, 7)), np.eye(7)]),
np.hstack([np.zeros((7, 7)), np.zeros((7, 7))])
])
self._B = np.vstack([np.zeros((7, 7)), np.eye(7)])
Q = 0.2 * np.diag([
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
1.0
])
R = np.eye(7)
def solve_lqr(A, B, Q, R):
P = solve_continuous_are(A, B, Q, R)
K = np.dot(np.dot(np.linalg.inv(R), B.T), P)
return K
self._K = solve_lqr(self._A, self._B, Q, R)
# self._Kp = 6*np.diag(np.array([1, 1, 1.5, 1.5, 1, 1, 1]))
# self._Kv = 5*np.diag(np.array([2, 2, 1, 1, 0.8, 0.3, 0.3]))
# self._Kp = 9*np.diag(np.array([4, 6, 4, 8, 1, 5, 1]))
# self._Kv = 5*np.diag(np.array([2, 3, 2, 4, 0.8, 1.5, 0.3]))
# WORKS WELL FOR TRAINING
self._Kp = 6 * np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
self._Kv = 6 * np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
# Tune down for testing except I didn't
# self._Kp = 5*np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
# self._Kv = 5*np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
if not self.register_callbacks(): sys.exit(1)
import time
import random
import pdb
import os
from baxter_core_msgs.msg import JointCommand
from baxter_pykdl import baxter_kinematics
rospy.init_node('baxter_kinematics')
#Ponemos RATE de mensaje
rate = rospy.Rate(
500) #10000 104 es la primera que funciona bien - Seria 0.1 ms o 10kHz
#Creamos objeto PUBLICADOR
pub = rospy.Publisher('/robot/limb/left/joint_command',
JointCommand,
queue_size=10)
limb = baxter_interface.Limb('left')
min_KP = 0
max_KP = 100
min_KD = 0.01
max_KD = 1
min_KI = 1
max_KI = 10
def individual():
#Crea individuo
a = [random.uniform(min_KP, max_KP) for i in [0, 1, 2, 3, 4, 5, 6]]
b = [random.uniform(min_KD, max_KD) for i in [7, 8, 9, 10, 11, 12, 13]]
c = [random.uniform(min_KI, max_KI) for i in [14, 15, 16, 17, 18, 19, 20]]
return a + b + c
def main():
print("Welcome to Sliders")
rospy.init_node('dev_joint_sliders')
rate = rospy.Rate(300)
relax = 2 # relax factor for thresholding goal
joint_tau = baxter_interface.settings.JOINT_ANGLE_TOLERANCE * relax
joy_in = Joy()
joy_in_axes = []
joy_in_btns = []
def _on_joy(data, joy_store):
joy_store.axes = list(data.axes)
joy_store.buttons = list(data.buttons)
print data.axes
robot = baxter_interface.RobotEnable()
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
joy_sub = rospy.Subscriber('/joy', Joy, _on_joy, joy_in)
print("waiting for first /joy message....")
def body_fun():
print ">>",
print(joy_in_axes)
baxter_dataflow.wait_for(lambda: len(joy_in.axes) > 0,
rate=10,
timeout=5.0,
body=body_fun)
joint_maxs = [1.701, 1.047, 3.054, 2.618, 3.059, 2.094, 3.059]
joint_mins = [-1.701, -2.147, -3.054, -0.050, -3.059, -1.571, -3.059]
inc_major = [0.1] * 7
inc_minor = [0.01] * 7
half_range = [1.0] * 7
mid_major = [0.0] * 7
flip_left = [-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0]
flip_right = [1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0]
for j in range(7):
inc_major[j] = (joint_maxs[j] - joint_mins[j]) / 128.0
inc_minor[j] = inc_major[j] / 128.0
half_range[j] = (joint_maxs[j] - joint_mins[j]) / 2.0
mid_major[j] = joint_mins[j] + half_range[j]
def in_out(in_axes, j_max, j_min, flip, a_max=1.0, a_min=-1.0):
pass
def inc_in(idx, in_major, in_minor, flip):
j_out = mid_major[idx] + flip * (half_range[idx] * in_major +
inc_major[idx] * in_minor)
# base = joint_maxs[idx] if flip < 0 else joint_mins[idx]
# j_out = base + flip * (inc_major[idx] * in_major
# + inc_minor[idx] * in_minor)
return j_out
idx_axes_major = [0, 1, 2, 3, 4, 5, 6]
idx_axes_minor = [8, 9, 10, 11, 12, 13, 14]
left_cmd = [0.0] * 7
right_cmd = [0.0] * 7
left.set_joint_position_speed(0.3)
right.set_joint_position_speed(0.3)
left.move_to_neutral()
right.move_to_neutral()
modes = ['mirror', 'left', 'right']
mode = 0
print("Sliding...")
while not rospy.is_shutdown():
joy_in_buttons = joy_in.buttons
joy_in_axes = joy_in.axes
for j in range(7):
left_cmd[j] = inc_in(j, joy_in_axes[idx_axes_major[j]],
joy_in_axes[idx_axes_minor[j]], flip_left[j])
right_cmd[j] = inc_in(j, joy_in_axes[idx_axes_major[j]],
joy_in_axes[idx_axes_minor[j]],
flip_right[j])
print(left_cmd)
print(right_cmd)
left.set_joint_positions(dict(zip(left.joint_names(), left_cmd)))
right.set_joint_positions(dict(zip(right.joint_names(), right_cmd)))
# right.move_to_joint_positions(dict(zip(right.joint_names(), right_cmd)), timeout=0.1)
rate.sleep()
def map_joystick(joystick):
"""
Maps joystick input to joint position commands.
@param joystick: an instance of a Joystick
"""
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lcmd = {}
rcmd = {}
#available joints
lj = left.joint_names()
rj = right.joint_names()
#abbreviations
jhi = lambda s: joystick.stick_value(s) > 0
jlo = lambda s: joystick.stick_value(s) < 0
bdn = joystick.button_down
bup = joystick.button_up
def print_help(bindings_list):
print("Press Ctrl-C to quit.")
for bindings in bindings_list:
for (test, _cmd, doc) in bindings:
if callable(doc):
doc = doc()
print("%s: %s" % (str(test[1][0]), doc))
bindings_list = []
bindings = (
((bdn, ['rightTrigger']), (grip_left.close, []), "left gripper close"),
((bup, ['rightTrigger']), (grip_left.open, []), "left gripper open"),
((bdn, ['leftTrigger']), (grip_right.close, []),
"right gripper close"),
((bup, ['leftTrigger']), (grip_right.open, []), "right gripper open"),
((jlo, ['leftStickHorz']), (set_j, [rcmd, right, rj, 0, 0.1]),
lambda: "right inc " + rj[0]),
((jhi, ['leftStickHorz']), (set_j, [rcmd, right, rj, 0, -0.1]),
lambda: "right dec " + rj[0]),
((jlo, ['rightStickHorz']), (set_j, [lcmd, left, lj, 0, 0.1]),
lambda: "left inc " + lj[0]),
((jhi, ['rightStickHorz']), (set_j, [lcmd, left, lj, 0, -0.1]),
lambda: "left dec " + lj[0]),
((jlo, ['leftStickVert']), (set_j, [rcmd, right, rj, 1, 0.1]),
lambda: "right inc " + rj[1]),
((jhi, ['leftStickVert']), (set_j, [rcmd, right, rj, 1, -0.1]),
lambda: "right dec " + rj[1]),
((jlo, ['rightStickVert']), (set_j, [lcmd, left, lj, 1, 0.1]),
lambda: "left inc " + lj[1]),
((jhi, ['rightStickVert']), (set_j, [lcmd, left, lj, 1, -0.1]),
lambda: "left dec " + lj[1]),
((bdn, ['rightBumper']), (rotate, [lj]), "left: cycle joint"),
((bdn, ['leftBumper']), (rotate, [rj]), "right: cycle joint"),
((bdn, ['btnRight']), (grip_left.calibrate, []), "left calibrate"),
((bdn, ['btnLeft']), (grip_right.calibrate, []), "right calibrate"),
((bdn, ['function1']), (print_help, [bindings_list]), "help"),
((bdn, ['function2']), (print_help, [bindings_list]), "help"),
)
bindings_list.append(bindings)
rate = rospy.Rate(100)
print_help(bindings_list)
print("Press Ctrl-C to stop. ")
while not rospy.is_shutdown():
for (test, cmd, doc) in bindings:
if test[0](*test[1]):
cmd[0](*cmd[1])
if callable(doc):
print(doc())
else:
print(doc)
if len(lcmd):
left.set_joint_positions(lcmd)
lcmd.clear()
if len(rcmd):
right.set_joint_positions(rcmd)
rcmd.clear()
rate.sleep()
return False
#!/usr/bin/env python
import rospy
import baxter_interface
rospy.init_node('ArmInit')
rospy.loginfo("Started...")
leftLimb = baxter_interface.Limb('left')
rightLimb = baxter_interface.Limb('right')
leftPoint = {
'left_s0': 0.5989,
'left_s1': 0.213,
'left_e0': -2.294,
'left_e1': 1.868,
'left_w0': -2.11,
'left_w1': -1.4734,
'left_w2': 3.289
}
rightPoint = {
'right_s0': -0.7497331092224122,
'right_s1': -0.14227671791381838,
'right_w0': 2.49501974864502,
'right_w1': -1.0657331511657715,
'right_w2': 2.489,
'right_e0': 1.974616766949463,
'right_e1': 1.8411604385559084
}
leftLimb.move_to_joint_positions(leftPoint)
rightLimb.move_to_joint_positions(rightPoint)
import Queue as Q
from positionControl import *
from taskFunctions import *
from std_msgs.msg import String
from baxter_core_msgs.msg import CollisionDetectionState
rospy.init_node('speechControl')
# Global Variables
rawCommand = ""
collisionState = False
lLimb = baxter.Limb('left')
rLimb = baxter.Limb('right')
lGripper = baxter.Gripper('left')
rGripper = baxter.Gripper('right')
# Text to speech engine
t2s = pyttsx.init()
voices = t2s.getProperty('voices')
t2s.setProperty('voice', 'english')
"""
for voice in voices:
print(voice.id)
"""
t2s.setProperty('rate', 150)
################## Definitions #####################
def __init__(self,
argv,
configFileName=None,
ikseedsFileName=None,
roadmapLeft=None,
roadmapRight=None,
leftElectricGripper=False,
rightElectricGripper=False):
self._lock = threading.RLock()
moveit_commander.roscpp_initialize(argv)
self._moveGroupPid = None
# wait for move_group action to become available to avoid timeout
# move_group_client = actionlib.SimpleActionClient('/move_group', MoveGroupAction)
# rospy.loginfo('[apc_grasping/MoveItInterface]: waiting for move_group action server')
# move_group_client.wait_for_server()
self._robotCommander = None
self._leftArmGroup = None
self._moveitScene = None
self._rightArmGroup = None
self._preempted = False
self._tfListener = utils.TFCache.TFCache()
self._objectsInScene = []
moveGroupAvailable = False
self._configurationStack = {'left_arm': [], 'right_arm': []}
# check for move group availability (the ugly way....)
while not moveGroupAvailable and not rospy.is_shutdown():
try:
self.checkMoveGroup(bResetScene=False)
moveGroupAvailable = True
except MoveItMissingException:
rospy.logwarn('Waiting for move_group')
self.sleep(0.5)
self._co_publisher = rospy.Publisher('/collision_object',
CollisionObject,
queue_size=1)
self._co_attach_publisher = rospy.Publisher(
'/attached_collision_object',
AttachedCollisionObject,
queue_size=1)
self._leftGlobalPlanner = utils.APCGlobalPathPlanner.APCGlobalPathPlanner(
'left_arm', self._leftArmGroup, self.checkPreemption)
self._rightGlobalPlanner = utils.APCGlobalPathPlanner.APCGlobalPathPlanner(
'right_arm', self._rightArmGroup, self.checkPreemption)
self._armSwitchControllerService = {}
rospy.wait_for_service('/apc/hacks/set_control_mode_left')
self._armSwitchControllerService['left_arm'] = rospy.ServiceProxy(
'/apc/hacks/set_control_mode_left', ControlMode)
rospy.wait_for_service('/apc/hacks/set_control_mode_right')
self._armSwitchControllerService['right_arm'] = rospy.ServiceProxy(
'/apc/hacks/set_control_mode_right', ControlMode)
rospy.wait_for_service('/pause_joint_trajectory_server_left')
rospy.wait_for_service('/pause_joint_trajectory_server_right')
self._pause_joint_trajectory_service_left = rospy.ServiceProxy(
'pause_joint_trajectory_server_left', Empty)
self._pause_joint_trajectory_service_right = rospy.ServiceProxy(
'pause_joint_trajectory_server_right', Empty)
# self._tfListener = tf.TransformListener()
self._leftGripper = APCBaxterGripper(baxter_interface.Gripper('left'),
electric=leftElectricGripper)
self._rightGripper = APCBaxterGripper(
baxter_interface.Gripper('right'), electric=rightElectricGripper)
self._leftArm = baxter_interface.Limb('left')
self._rightArm = baxter_interface.Limb('right')
self._head = baxter_interface.Head()
self._namedConfigurations = {}
if roadmapLeft is not None:
rospy.loginfo('Loading roadmap for left arm from path ' +
roadmapLeft)
rospy.loginfo('Note, this may take a while')
self._leftGlobalPlanner.readRoadmap(roadmapLeft)
rospy.loginfo('Roadmap successfully loaded!')
if roadmapRight is not None:
rospy.loginfo('Loading roadmap for right arm from path ' +
roadmapRight)
rospy.loginfo('Note, this may take a while')
self._rightGlobalPlanner.readRoadmap(roadmapRight)
rospy.loginfo('Roadmap successfully loaded!')
self._seedIks = {}
if ikseedsFileName is not None:
try:
ikseedFile = open(ikseedsFileName, 'r')
self._seedIks = yaml.load(ikseedFile)
ikseedFile.close()
except IOError as err:
rospy.logerr('Could not load ik seeds file: ' + repr(err))
if 'left_arm' not in self._seedIks or self._seedIks['left_arm'] is None:
self._seedIks['left_arm'] = {}
if 'right_arm' not in self._seedIks or self._seedIks[
'right_arm'] is None:
self._seedIks['right_arm'] = {}
if configFileName is not None:
try:
configFile = open(configFileName, 'r')
self._namedConfigurations = yaml.load(configFile)
configFile.close()
except IOError as err:
rospy.logerr('Could not load configurations from file ' +
configFileName)
if 'left_arm' not in self._namedConfigurations:
self._namedConfigurations['left_arm'] = {}
if 'right_arm' not in self._namedConfigurations:
self._namedConfigurations['right_arm'] = {}
rospy.loginfo('MoveItInterface created.')
def main():
# If you want to debug, uncomment the next line.
ipdb.set_trace()
# Initialize node
rospy.init_node('example3_kdl')
# Call routine to enable the robot
rs = enable_Baxter()
## Params
loop = rospy.Rate(rate)
dp = zeros((3, 1))
# Create Limb Objects
rLimb = baxter_interface.Limb('right')
#lLimb=baxter_interface.Limb('left')
# Create Kinematic Objects
rKin = baxter_kinematics('right')
#lKin = baxter_kinematics('left')
# Get Joint names
#jNamesR_ordrered=rLimb.joint_names()
jNamesR = [
'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2', 'right_e0',
'right_e1'
]
# Set Vector to some goal to exploit redundancy.
# A. Vector pointing to mid-range of joint angles
if dq_to_ctr_mode:
upper_limits = [0.890, 1.047, 3.028, 2.618, 3.059, 2.094,
3.059] # s0s1e0e1w0w1w2
lower_limits = [-2.461, -2.147, -3.028, -0.052, -3.059, -1.571, -3.059]
mid_range = [(upper_limits[i] - lower_limits[i]) / 2 for i in range(7)]
q_red = matrix([
upper_limits[i] - mid_range[i] for i in range(7)
]).T # This is in effect a q_dot, a velocity of joint angles.
else:
# B. Vector with a simple
q_red = matrix(
'0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0'
).T # Select the joint you want to update in this 7x1 col vector
for i in range(
len(q_red)
): # Identify the joint that is non-zero and save that index.
if q_red[i, 0] != 0.0:
idx = i # Matrix indeces start from zero
# Set a reference pose: [x,y,z,vx,vy,vz,w]. This one is near the home position of arm.
# Home joint angle position for baxter_world_home.launch is approximately: [0.58,-0.19,0.23,-0.11,0.99,0.01,0.02]
ref_pose = {
'position': [0.70, -0.2, 0.23],
'orientation': [-0.12, 0.99, 0.01, 0.01]
}
ref_pos = matrix(ref_pose['position']).T
#ref_rot=matrix(ref_pose['orientation'])
print 'The reference pos is: '
print(ref_pos)
# Right Arm
while not rospy.is_shutdown():
print 'example3_kdl for the right arm...'
# 1. Get the current angles and jacobian as numpy matrices
rAngles = rLimb.joint_angles() # returns as s0s1w0w1w2e0e1
rAnglesM = matrix(rAngles.values()).T
jac_n = rKin.jacobian(
) # wrt end-effector frame. TODO: change to world frame.
# 2. Compute the forward Kinematics using the Jacobian: del_p=J(q)del_q
curr_pos = rLimb.endpoint_pose()
curr_pos = matrix(curr_pos['position']).T
print 'The current position is:\t\t The reference positions is:'
for i in range(len(curr_pos)):
print repr(curr_pos[i, 0]).rjust(10), repr(ref_pos[i, 0]).rjust(40)
# Position Mode
if pos_mode:
# 3. Compute the error between reference and current positions
del_p_error = Kp * (ref_pos - curr_pos[0:3])
# 4. Get a scalar distance (vector norm) to more easily interpret the error
dp_norm = norm(del_p_error)
print 'the dpnorm is %f' % dp_norm
# 5. Extract the translation Jacobian
jTrans = jac[0:3, 0:7] # when slicing go one dim past end
# 6. Compute the dq using dp_error
del_q = (pinv(jTrans)) * del_p_error # in terms of dp change
# 7. Add this dq to current joint Angles
ref_anglesM = del_q + rAnglesM
ref_angles = ref_anglesM.ravel().tolist()[
0] # change back to list.[0] b/c it returns nested list.
ref_angles = dict(zip(jNamesR, ref_angles))
# 8. Move the Limb wout working with the null space
#rLimb.move_to_joint_positions(ref_angles) # need to check that indeces are in the right order
rLimb.set_joint_positions(ref_angles)
# Velocity Mode
else:
# 3. Compute the error between reference and current positions
del_p_error = Kv * (ref_pos - curr_pos[0:3])
for i in range(len(del_p_error)):
dp[i] = del_p_error[i] / dt
# 4. Get a scalar distance (vector norm) to more easily interpret the error
dp_norm = norm(del_p_error)
print 'the dpnorm is %f' % dp_norm
# 5. Extract the translation Jacobian
jTrans = jac[0:3, 0:7] # when slicing go one dim past end
# 6. Compute the dq using dp_error
#del_q=(pinv(jTrans))*del_p_error # in terms of dp change
dq = (pinv(jTrans)) * dp # in terms of velocity
# 7. Add this dq to current joint Angles
ref_anglesM = (dt * dq) + rAnglesM # Covert from speed to distance
ref_angles = ref_anglesM.ravel().tolist()[
0] # change back to list.[0] b/c it returns nested list.
ref_angles = dict(zip(jNamesR, ref_angles))
# 8. Move the Limb wout working with the null space
#rLimb.move_to_joint_positions(ref_angles) # need to check that indeces are in the right order
rLimb.set_joint_positions(ref_angles)
if null_space_comp:
# 9. Compute the kernel (null space) for the translational Jacobian of Baxter (3x6) and the pseudo-inverse
jNull = matrix(null(jTrans))
jNull_inv = matrix(pinv(jNull))
# Test to make sure that the output of the translational Jacobian(3,7) times the kernel(7,4) is the zero matrix(3,4):
z = jTrans * jNull
print '\nTest to make sure that the output of the translational Jacobian(3,7) times the kernel(7,4), is the zero matrix(3,4):\njTrans*jNull is: '
print z
# 10. Compute direction of motion.
# If dqt_to_ctr, then compute error from center of joint range: desired - actual
# Scale the error so that it is appropriate for the Jacobian Null Space.
if dq_to_ctr_mode:
dq_ref = Kq * (q_red - ref_anglesM)
print '\nCenter Angles\t\t Current Angles\t\t\t Error'
for i in range(len(q_red)):
print repr(q_red[i, 0]).rjust(7), repr(
ref_anglesM[i, 0]).rjust(33), repr(dq_ref[i,
0]).rjust(29)
else:
dq_ref = q_red
print '\nThe error vector in radians is:'
print(dq_ref)
# Compute the norm
dq_ref_norm = norm(dq_ref)
print 'And, the norm of our error reference is: '
print(dq_ref_norm)
# 11. Project this error to the nullspace via NN*dq_to_ctr
null_proj = jNull * jNull_inv
dq_null = null_proj * dq_ref
print '\nThen, this error vector, projected unto the nullspace dq_null is:'
print dq_null
print ''
# 12. Scale dq_null.
# If you are only changing a single joint, the projection will modify that dimensions of that joint.
# We can do a scaling mechanism to reset that specific joint value to our original goal.
if single_joint_update_mode:
for i in range(len(dq_null)):
dq_null[i, 0] = (dq_null[i, 0] / dq_null[idx, 0]) * q_red[
idx,
0] # for loop index starts at 1 but the matrix index at 0
# 13. If norm is less than 0.00001 then add
print 'If we multipy dq_null x Jacobian and compute the norm, it should be zero...'
zero = norm(jTrans * dq_null)
print(zero)
# 14. Only move the joints if the computed dq_null in fact gives a small (near zero) output. Otherwise
# do not move the joint angles. This results in a much more stable motion.
if zero < 0.00001:
ref_null_anglesM = ref_anglesM + dq_null
# 12. Convert back to list and move
ref_null_angles = ref_null_anglesM.ravel().tolist()[
0] # change back to list.[0] b/c it returns nested list.
ref_null_angles = dict(zip(jNamesR, ref_null_angles))
#rLimb.set_joint_positions(ref_null_angles)
rLimb.move_to_joint_positions(ref_null_angles)
# Set the loop speed
loop.sleep()
# Clear the screen for easier reading
os.system('clear')
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(shutdown)
return rs
def main():
rospy.init_node('kinect_test')
vive_data = []
rate = rospy.Rate(30)
rospy.Subscriber("unity_locations", String, vive_callback)
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
left_kin = baxter_kinematics('left')
right_kin = baxter_kinematics('right')
print('Program Starting! 10 Seconds to move to initial pose')
rospy.sleep(6.0)
print('Pose Acquired, Demonstrator must hold this position for now!!!!')
for i in vive:
vive_data.append(float(i))
human_data = np.array(vive_data)
print "human_data"
print human_data
c1 = human_data[0:7]
c2 = human_data[7:14]
head = human_data[14:21]
c1[0] = c1[0] - head[0]
c1[1] = c1[1] - head[1]
c1[2] = c1[2] - head[2]
c2[0] = c2[0] - head[0]
c2[1] = c2[1] - head[1]
c2[2] = c2[2] - head[2]
human_data = np.concatenate((c1, c2))
human_data = np.reshape(human_data, (1,14))
print human_data
right_json_file = open('right_nn_without_head.json', 'r')
right_loaded_model_json = right_json_file.read()
right_json_file.close()
right_joint_nn_model = model_from_json(right_loaded_model_json)
# load weights into new model
right_joint_nn_model.load_weights("right_nn_without_head_wt.h5")
right_joint_nn_model.compile(loss = 'mean_squared_error', optimizer='sgd')
right_predicted_joints = right_joint_nn_model.predict(human_data)[0]
print right_predicted_joints
left_neutral_position = {'left_s0':0.948767, 'left_s1':0.901981, 'left_e0':-1.456131, 'left_e1':0.541495, 'left_w0':0.887408, 'left_w1':0.488189, 'left_w2':-2.937190}
right_neutral_position = {'right_s0':-1.039272, 'right_s1':0.868233, 'right_e0':1.078005, 'right_e1':0.537660, 'right_w0':-3.045335, 'right_w1':-0.272282, 'right_w2':-1.091811}
left.move_to_joint_positions(left_neutral_position)
print "left completed"
right.move_to_joint_positions(right_neutral_position)
right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]})
#print 'Want Robot Ready to Move?[y/n]',
#userInput = sys.stdin.readline().strip()
#if(userInput == 'y'):
#left.move_to_joint_positions(left_neutral_position)
#print "left completed"
#right.move_to_joint_positions(right_neutral_position)
#right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]})
#else:
#print("Program exiting...")
#return
#print('Attempt real time tracking?[y/n]')
#if(userInput != 'y'):
# print("Program exiting...")
# return
pub_right = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size=10)
pub_left = rospy.Publisher('/robot/limb/left/joint_command', JointCommand, queue_size=10)
print('Realtime Tracking Starting! Demonstrator can move very slowly now')
while not rospy.is_shutdown():
vive_data = []
for i in vive:
vive_data.append(float(i))
human_data = np.array(vive_data)
c1 = human_data[0:7]
c2 = human_data[7:14]
head = human_data[14:21]
c1[0] = c1[0] - head[0]
c1[1] = c1[1] - head[1]
c1[2] = c1[2] - head[2]
c2[0] = c2[0] - head[0]
c2[1] = c2[1] - head[1]
c2[2] = c2[2] - head[2]
human_data = np.concatenate((c1, c2))
human_data = np.reshape(human_data, (1,14))
print human_data
right_predicted_joints = right_joint_nn_model.predict(human_data)[0]
#right_end_effector = right_kin.forward_position_kinematics({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]})
#print "end effector:"
#print right_end_effector
#with open("test_end_effector_vive.csv", 'a') as myfile:
# wr = csv.writer(myfile, quoting=csv.QUOTE_ALL)
# wr.writerow(np.concatenate((np.array(right_end_effector), human_data[0])))
print "predicted joints:"
print right_predicted_joints
#right.move_to_joint_positions({'right_s0': right_predicted_joints[0],'right_s1': right_predicted_joints[1],'right_e0': right_predicted_joints[2],'right_e1': right_predicted_joints[3],'right_w0': right_predicted_joints[4],'right_w1':right_predicted_joints[5],'right_w2': right_predicted_joints[6]})
command_msg = JointCommand()
command_msg.mode = JointCommand.POSITION_MODE
command_msg.command = [right_predicted_joints[i] for i in xrange(0, 6)]
command_msg.names = ['right_s0', 'right_s1','right_e0','right_e1','right_w0','right_w1', 'right_w2']
pub_right.publish(command_msg)
rate.sleep()
def Step5(q1, q2, q3, q4, s):
limb = baxter_interface.Limb('right')
angles = limb.joint_angles()
def ik_test_Luky(limb, poza):
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
poses = {
'right':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=poza['position'].x + 0.2,
y=poza['position'].y +
(0.2 * tan(s)), #-0.25, #-0.2837,
z=poza['position'].z),
orientation=Quaternion(
x=q1,
y=q2,
z=q3,
w=q4,
),
),
),
}
ikreq.pose_stamp.append(poses[limb])
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: '******',
ikreq.SEED_CURRENT: 'Current Joint Angles',
ikreq.SEED_NS_MAP: 'Nullspace Setpoints',
}.get(resp_seeds[0], 'None')
print("\nSUCCESS - 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))
#print "\nIK Joint Solution:\n", limb_joints
print "------------------"
#print "Response Message:\n", resp
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return limb_joints
def main():
# If you want to debug, uncomment the next line.
# pdb.set_trace()
# Initialize node
rospy.init_node('example_baxter_kins_right')
# Call routine to enable the robot
rs = enable_Baxter()
# Set a ROS Rate for the while loop at 1Hz
loop = rospy.Rate(5)
# Create Limb Objects
rLimb = baxter_interface.Limb('right')
lLimb = baxter_interface.Limb('left')
# Create Kinematic Objects
rKin = baxter_kinematics('right')
lKin = baxter_kinematics('left')
# Angles and Kinematics
print 'Printing joing angles from baxter_interface, fkine, ikine. Please move baxter\'s joints'
# Right Arm
while not rospy.is_shutdown():
print 'Right Arm'.center(35, '*')
print 'Joint Angles from Baxter Interface'
rAngles = rLimb.joint_angles() # Angles
print 's0\ts1\te0\te1\tw0\tw1\tw2' # Angle sequence
print(round(rAngles['right_s0'], 4), round(rAngles['right_s1'], 4),
round(rAngles['right_e0'], 4), round(rAngles['right_e1'], 4),
round(rAngles['right_w0'],
4), round(rAngles['right_w1'],
4), round(rAngles['right_w2'], 4))
print '' # Empty line
print 'Pose from FKins...'
pose = rKin.forward_position_kinematics() # Fkine
print(
'x: ',
round(pose[0], 2),
'y: ',
round(pose[1], 2),
'z: ',
round(pose[2], 2),
'vx: ',
round(pose[3], 2),
'vy: ',
round(pose[4], 2),
'vz: ',
round(pose[5], 2),
'w: ',
round(pose[6], 2),
)
print ''
# Get End pose, without orientation
print 'Cartesian End-Effector Pose'
rPose = rLimb.endpoint_pose()
print(
'x: ',
round(rPose['position'][0], 2),
'y: ',
round(rPose['position'][1], 2),
'z: ',
round(rPose['position'][2], 2),
'vx: ',
round(rPose['orientation'][0], 2),
'vy: ',
round(rPose['orientation'][1], 2),
'vz: ',
round(rPose['orientation'][2], 2),
'w: ',
round(rPose['orientation'][3], 2),
)
print ''
#print 'The RPY is: ', quat_to_angle(Quaternion(*orientation))
print 'Joint Angles from IKins...'
rIKin = rKin.inverse_kinematics(rPose['position'],
rPose['orientation'])
print round(rIKin[0],
4), round(rIKin[1], 4), round(rIKin[2], 4), round(
rIKin[3], 4), round(rIKin[4],
4), round(rIKin[5],
4), round(rIKin[6], 4)
print ''
# Set the loop speed
loop.sleep()
# Clear the screen for easier reading
os.system('clear')
# Set rospy to execute a shutdown function when exiting
rospy.on_shutdown(shutdown)
return rs
def robot_setup(self):
baxter_interface.RobotEnable().enable()
self.robot = baxter_interface.Limb('right')
self.robot.set_joint_position_speed(0.5)
self.talk = actionlib.SimpleActionClient('/speak', maryttsAction)
feedback.feedback = 1
result = BaxterActionResult()
result.result = 1
print "Right hand moving !"
angles = dict()
wave_1 = {
'right_s0': goal.coordinates[0],
'right_s1': goal.coordinates[1],
'right_e0': goal.coordinates[2],
'right_e1': goal.coordinates[3],
'right_w0': goal.coordinates[4],
'right_w1': goal.coordinates[5],
'right_w2': goal.coordinates[6]
}
limbR.move_to_joint_positions(wave_1, 15, goal.precision)
server.publish_feedback(feedback)
server.set_succeeded(result)
rospy.sleep(0.1)
limbR = baxter_interface.Limb('right')
server = actionlib.SimpleActionServer('/Rhand_server', BaxterActionAction,
callback, False)
server.start()
print("right hand ready")
rospy.spin()
math.sqrt(crossVector[2]**2))
crossVector_norm[0] = crossVector[0] / magnitude
crossVector_norm[1] = crossVector[1] / magnitude
crossVector_norm[2] = crossVector[2] / magnitude
print 'normalized cross vector', crossVector_norm
getNormalVector2()
print("MIM tutorial: forward kinematics.")
# Initialising ROS node
rospy.init_node("SSE_forward_kinematics")
###################### INSERT YOUR CODE HERE
# Create a "Limb" instance called "left_arm" linked to Baxter's left limb
left_arm = baxter_interface.Limb("left")
# Create a "pose" variable which holds the output of endpoint_pose()
pose = left_arm.endpoint_pose()
angles = left_arm.joint_angles()
print angles
lcmd = {
'left_w0': -0.381669966846,
'left_w1': 0.22169442983,
'left_w2': -0.98,
'left_e0': -0.751267090009,
'left_e1': 1.02009722278,
'left_s0': 0.335558296967,
'left_s1': -0.214757310059
}
robot = moveit_commander.RobotCommander()
# Add in objects
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.8
p.pose.position.y = 0.025
p.pose.position.z = -0.6
scene.add_box("table", p, (0.75, 1.25, 0.68))
traj_controller = Trajectory_Controller()
ikright = Inverse_Kinematics(limb="right")
right_arm = baxter_interface.Limb('right')
left_arm = baxter_interface.Limb('left')
while not rospy.is_shutdown():
rospy.wait_for_service('joint_values')
try:
joint_service_proxy = rospy.ServiceProxy('joint_values',
JointValues)
joint_resp = joint_service_proxy()
if joint_resp.new_values:
if "joints" in joint_resp.val_type:
joints = dict(
zip(joint_resp.joint_names, joint_resp.joint_values))
try:
both_arms_group.set_joint_value_target(joints)
traj_controller.push(both_arms_group.plan())
class MoveArms(SmokeTest):
"""
Move both arms to numerous joint positions.
"""
def __init__(self, name='MoveArms'):
super(MoveArms, self).__init__(name)
def start_test(self):
"""Runs MoveArms Smoke Test.
"""
def move_thread(limb, angle, queue, timeout=15.0):
"""
Threaded joint movement allowing for simultaneous joint moves.
"""
try:
limb.move_to_joint_positions(angle, timeout)
queue.put(None)
except Exception, exception:
queue.put(traceback.format_exc())
queue.put(exception)
try:
print "Enabling robot..."
self._rs.enable()
print "Test: Create Limb Instances"
right = baxter_interface.Limb('right')
left = baxter_interface.Limb('left')
left_queue = Queue.Queue()
right_queue = Queue.Queue()
# Max Joint Range (s0, s1, e0, e1, w0, w1, w2)
# ( 1.701, 1.047, 3.054, 2.618, 3.059, 2.094, 3.059)
# Min Joint Range (e0, e1, s0, s1, w0, w1, w2)
# (-1.701, -2.147, -3.054, -0.050, -3.059, -1.571, -3.059)
joint_moves = (
[0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0],
[0.5, -0.8, 2.8, 0.15, 0.0, 1.9, 2.8],
[-0.1, -0.8, -1.0, 2.5, 0.0, -1.4, -2.8],
[0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0],
)
for move in joint_moves:
print "Test: Moving to Joint Positions: ",
print ", ".join("%.2f" % x for x in move)
left_thread = threading.Thread(
target=move_thread,
args=(left, dict(zip(left.joint_names(),
move)), left_queue))
right_thread = threading.Thread(
target=move_thread,
args=(right, dict(zip(right.joint_names(),
move)), right_queue))
left_thread.daemon = True
right_thread.daemon = True
left_thread.start()
right_thread.start()
baxter_dataflow.wait_for(
lambda: not (left_thread.is_alive() or right_thread.
is_alive()),
timeout=20.0,
timeout_msg=("Timeout while waiting for arm move threads"
" to finish"),
rate=10,
)
left_thread.join()
right_thread.join()
result = left_queue.get()
if not result is None:
raise left_queue.get()
result = right_queue.get()
if not result is None:
raise right_queue.get()
rospy.sleep(1.0)
print "Disabling robot..."
self._rs.disable()
self.result[0] = True
rospy.sleep(5.0)
except Exception:
self.return_failure(traceback.format_exc())
Range,
)
from trac_ik_python.trac_ik import IK
from baxter_interface import CHECK_VERSION
import tf2_ros
import tf2_geometry_msgs
import random as rd
rospy.init_node("marker_ik_example_movement", disable_signals=True)
global tf_buffer
tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) #tf buffer length
#set limb speed
right = baxter_interface.Limb('right')
right.set_joint_position_speed(1.0)
left = baxter_interface.Limb('left')
left.set_joint_position_speed(1.0)
#prev time for marker callback
global prev_time
prev_time = 0
#range array for laser range finder
global range_array
range_array = collections.deque(5 * [(0.0, 0.0)], 5)
#endpoint array of arm positions
global endpoint_array
endpoint_array = []
#array of markers
global marker_array
def main():
rospy.init_node('execute')
rs = baxter_interface.RobotEnable(baxter_interface.CHECK_VERSION)
rs.enable()
# retrieve images
global current_image
def update_image(msg):
global current_image
current_image = PIL_Image.frombytes('RGBA', (msg.width, msg.height),
msg.data)
# print msg.width, msg.height, msg.is_bigendian, msg.step, msg.encoding
rospy.Subscriber('/cameras/left_hand_camera/image', Image, update_image)
# model
width = 128
checkpoint_dir = 'checkpoints-dev-rgb-4-max'
grasp_class_prediction, logit, grasp_image_ph, keep_prob_ph = build_model(
width)
saver = tf.train.Saver(max_to_keep=5, keep_checkpoint_every_n_hours=1)
arm = baxter_interface.Limb('left')
arm.move_to_neutral()
gripper = baxter_interface.Gripper('left')
gripper.calibrate()
# grasp crop
crop_center_x = 330
crop_center_y = 160
grasp_class_threashold = 0.5
scale = 1.0
crop_width = width * scale
crop_box = (crop_center_x - crop_width / 2, crop_center_y - crop_width / 2,
crop_center_x + crop_width / 2, crop_center_y + crop_width / 2)
# grasp workspace
x0 = 0.81
y0 = 0.25
delta = 0.04
initial_z = 0.1
bound_z = -0.165
grasp_class_threashold = 0.5
pub = rospy.Publisher('/robot/xdisplay', Image, queue_size=1)
global force
def display_gripper_state(msg):
global force
force = msg.force
rospy.Subscriber('/robot/end_effector/left_gripper/state',
EndEffectorState, display_gripper_state)
with tf.Session() as sess:
restore_vars(saver, sess, checkpoint_dir)
attemp = 0
while True:
# sample a grasp
dx = np.random.rand() * (2. * delta) - delta
dy = np.random.rand() * (2. * delta) - delta
target_theta = (np.random.rand() * 2. - 1.) * 3.059
target_x = x0 + dx
target_y = y0 + dy
# move to the grasp location
execute_linear(arm, target_x, target_y, initial_z, target_theta)
# predict grasp
crop = np.array(
current_image.crop(crop_box).resize((width, width)))[:, :, :3]
grasp_pred = grasp_class_prediction.eval(session=sess,
feed_dict={
grasp_image_ph:
crop.reshape(
(1, width, width,
3)),
keep_prob_ph:
1.,
})
# display image
draw = PIL_ImageDraw.Draw(current_image)
draw.text(crop_box[:2], 'prob: %.5f' % grasp_pred[0, 1])
draw.text((20, 20), 'grasp force: %.5f' % force)
if grasp_pred[0, 1] > grasp_class_threashold:
draw.rectangle(crop_box, outline=(0, 255, 0))
else:
draw.rectangle(crop_box, outline=(0, 0, 255))
msg = Image(
header=Header(
stamp=rospy.Time.now(),
frame_id='base',
),
width=640,
height=400,
step=640 * 4,
encoding='bgra8',
is_bigendian=0,
data=current_image.tobytes(),
)
pub.publish(msg)
if grasp_pred[0, 1] > grasp_class_threashold:
execute_planar_grasp(arm,
gripper,
initial_z,
bound_z,
target_theta,
lower_to_drop=0.05)
attemp += 1
def main():
rospy.sleep(.2)
global camdata
#print "camdata is" ,camdata
global right
global lg
global left
#subscribe to all necessary sources
tf_listener = tf2_ros.TransformListener(tf_buffer)
#range finder
rospy.Subscriber('/robot/range/left_hand_range/state',
Range,
callback_range,
queue_size=1)
#Alvar markers
rospy.Subscriber('/ar_pose_marker',
AlvarMarkers,
callback_marker,
queue_size=1)
#endpoint state
rospy.Subscriber('/robot/limb/left/endpoint_state',
EndpointState,
callback_endpoint,
queue_size=1)
#set speed
right = baxter_interface.Limb('right')
right.set_joint_position_speed(1.0)
left = baxter_interface.Limb('left')
left.set_joint_position_speed(1.0)
camdata = Point()
#left gripper
lg = baxter_interface.Gripper('left')
#calibrate gripper
if lg.calibrated() == False:
lg.calibrate()
#rg.calibrate()
lg.open()
quit = "n"
#starting points for arm
pos = Point(x=0.50, y=0.182539067108, z=0.0998704377885)
quat = Quaternion(x=0.0, y=1.0, z=0.0, w=0.0)
pos_start = Point(x=0.578951563602, y=0.182539067108, z=0.3998704377885)
# hard coded joint postion used for demos, not needed
# high_joints = {'left_w0': 0.20747090156150222, 'left_w1': 1.3169225063996277,
# 'left_w2': -0.37314082665312687, 'left_e0': -0.7098496095939753,
# 'left_e1': 1.5523885573400387, 'left_s0': -0.4111068511532909,
# 'left_s1': -1.2532623037023831}
joints = ik_solve('left', pos_start, quat)
left.move_to_joint_positions(joints)
# Uncomment as apppropriate, find_surface takes some time so its best to
# re-use this value for demos/debugging/repeated runs
# Find surface_z and set with rangefinder
surface_z = find_surface(pos_start, quat)
# lab box height
#surface_z = -0.314529563082
# lab small table height
#surface_z = -0.0791295619731
#pre pose height
pre_pose_high = Point(x=0.50, y=0.272539067108, z=0.3998704377885)
print "pos at start"
print pos_start.z
print surface_z
global poseglobal
rd.seed(5)
# Open grippers
lg.open()
# Main Grab loop
for i in range(0, 100):
print i
# Allow time for grippers to drop a block if holding on
rospy.sleep(0.5)
# Move arm to a start position above the surface
joints = ik_solve('left', pos, quat)
if surface_z > -0.10:
high_joints = ik_solve('left', pre_pose_high, quat)
left.move_to_joint_positions(high_joints)
else:
left.move_to_joint_positions(joints)
# Wait to for some position data to come in from a marker topic
rospy.sleep(2)
pos1 = update_arm_pos(pos)
# construct a PoseStamped from pos1
pose_sm = Point(x=pos1.x + 0.05, y=pos1.y, z=surface_z + 0.15)
ps = makePoseStamped(pose_sm, quat, "base")
# wait for more data move to pre-grap pose for better accuracy
rospy.sleep(1)
pos1 = update_arm_pos(pos)
pose_sm = Point(x=pos1.x + 0.05, y=pos1.y, z=surface_z + 0.16)
joints = ik_solve('left', pose_sm, quat)
left.move_to_joint_positions(joints)
# wait for data in pre-grap pose, move to grab pose
rospy.sleep(1)
pose_grip = Point(x=pos1.x + 0.01, y=pos1.y, z=surface_z + 0.12)
joints = ik_solve('left', pose_grip, quat)
left.move_to_joint_positions(joints)
# Grab block
lg.close()
rospy.sleep(0.5)
# Move to randomised drop position magic numbers represent guesses for
# sensible x and y bounds in base frame reference
dropx = rd.uniform(0.40, 0.60)
dropy = rd.uniform(0.18, 0.35)
pose_drop = Point(x=dropx, y=dropy, z=surface_z + 0.15)
joints = ik_solve('left', pose_drop, quat)
left.move_to_joint_positions(joints)
rospy.sleep(2)
# Open grippers
lg.open()
print "clear"
last_left_ar_posns.clear()
def map_keyboard():
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
def set_j(limb, joint_name, delta):
current_position = limb.joint_angle(joint_name)
joint_command = {joint_name: current_position + delta}
limb.set_joint_positions(joint_command)
bindings = {
# key: (function, args, description)
'9': (set_j, [left, lj[0], 0.1], "left_s0 increase"),
'6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"),
'8': (set_j, [left, lj[1], 0.1], "left_s1 increase"),
'7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"),
'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"),
'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"),
'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"),
'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"),
'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"),
'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"),
'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"),
'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"),
'.': (set_j, [left, lj[6], 0.1], "left_w2 increase"),
'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
',': (grip_left.close, [], "left: gripper close"),
'm': (grip_left.open, [], "left: gripper open"),
'/': (grip_left.calibrate, [], "left: gripper calibrate"),
'4': (set_j, [right, rj[0], 0.1], "right_s0 increase"),
'1': (set_j, [right, rj[0], -0.1], "right_s0 decrease"),
'3': (set_j, [right, rj[1], 0.1], "right_s1 increase"),
'2': (set_j, [right, rj[1], -0.1], "right_s1 decrease"),
'r': (set_j, [right, rj[2], 0.1], "right_e0 increase"),
'q': (set_j, [right, rj[2], -0.1], "right_e0 decrease"),
'e': (set_j, [right, rj[3], 0.1], "right_e1 increase"),
'w': (set_j, [right, rj[3], -0.1], "right_e1 decrease"),
'f': (set_j, [right, rj[4], 0.1], "right_w0 increase"),
'a': (set_j, [right, rj[4], -0.1], "right_w0 decrease"),
'd': (set_j, [right, rj[5], 0.1], "right_w1 increase"),
's': (set_j, [right, rj[5], -0.1], "right_w1 decrease"),
'v': (set_j, [right, rj[6], 0.1], "right_w2 increase"),
'z': (set_j, [right, rj[6], -0.1], "right_w2 decrease"),
'c': (grip_right.close, [], "right: gripper close"),
'x': (grip_right.open, [], "right: gripper open"),
'b': (grip_right.calibrate, [], "right: gripper calibrate"),
}
while not done and not rospy.is_shutdown():
c = getch()
if c:
#catch Esc or ctrl-c
print "c:",c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
cmd[0](*cmd[1])
print("command: %s" % (cmd[2],))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-l', '--limb', required=True, choices=['left', 'right'],
help='send joint trajectory to which limb'
)
args = parser.parse_args(rospy.myargv()[1:])
print args
limb = args.limb
print("Initializing node... ")
rospy.init_node("super_stacker_%s" % (limb,))
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
rs.enable()
#Calibrate gripper
gripper_if = baxter_interface.Gripper(limb)
if not gripper_if.calibrated():
print "Calibrating gripper"
gripper_if.calibrate()
limbInterface = baxter_interface.Limb(limb)
iksvc, ns = ik_command.connect_service(limb)
rate = rospy.Rate(100)
# Get goal poses of each object in the scene
dc = DepthCaller(limb)
# Subscribe to estimate_depth
# Move to pose published by estimate_depth
while (not dc.done) and (not rospy.is_shutdown()):
rate.sleep()
#pass
# Subscribe to object_finder and start visual servoing/grasping
# The stack pose is the pose of the smallest object with a z-offset
# accounting for the height of the object (this assumption makes
# it really important that the segmentation is accurate)
stack_pose = [0.594676466827, -0.296644499519, -0.0322744943164, 0.971805911045, -0.22637170004, 0.065946440385, 0.000437813100735]
#stack_pose = dc.object_poses[-1]
#stack_pose = incrementPoseZ(stack_pose, object_height)
#dc.object_poses.pop(len(dc.object_poses)-1)
for pose in dc.object_poses:
ik_command.service_request(iksvc, pose, limb, blocking=True)
vc = VisualCommand(iksvc, limb)
vc.subscribe()
while (not vc.done) and (not rospy.is_shutdown()):
rate.sleep()
ik_command.service_request(iksvc, stack_pose, limb, blocking=True)
#limbInterface.move_to_joint_positions(stack_pose)
# Let go
gripper_if.open(block=True)
stack_pose = incrementPoseZ(pose, object_height)
cmd['left_e1'] = dp[3]
cmd['left_w0'] = dp[4]
cmd['left_w1'] = dp[5]
cmd['left_w2'] = dp[6]
# for i,joint in enumerate(leftarm.joint_names()):
# cmd[joint]=dp[i]
leftarm.set_joint_velocities(cmd)
if __name__ == "__main__":
rospy.init_node('baxter_kin')
print 'initial node successd'
rs = baxter_interface.RobotEnable(CHECK_VERSION)
rs.enable()
print 'enable baxter successed'
leftarm = baxter_interface.Limb('left')
print 'move leftarm to the initial position'
leftarm.move_to_neutral()
print '****** move leftarm in x direction ******'
count = 0
while count < 500:
Movejac()
count = count + 1
print 'move leftarm to the initial position'
leftarm.move_to_neutral()
count = 0
print '****** move in Null space ******'
while count < 100:
nullspace()
def Step2():
limb1 = baxter_interface.Limb('left')
limb = baxter_interface.Limb('right')
limb.set_joint_position_speed(1.0)
wave_1 = {
'right_s0': 0.7144515511413575,
'right_s1': -0.6699661083435059,
'right_w0': -1.740301201867676,
'right_w1': -1.570796325,
'right_w2': 0.018791264630126956,
'right_e0': 0.06250971703491211,
'right_e1': 0.704097180834961
}
wave_2 = {
'left_w0': 3.03383049977417,
'left_w1': -1.5079031127685547,
'left_w2': -0.21207284368286133,
'left_e0': 0.15301458341674806,
'left_e1': 0.6707330987365723,
'left_s0': -0.9295923563964844,
'left_s1': -0.7221214550720215
}
wave_3 = {
'right_s0': 0.7857816576965333,
'right_s1': -0.1955825502319336,
'right_w0': -2.869694555657959,
'right_w1': 1.609912835046387,
'right_w2': -3.0591411827453614,
'right_e0': -1.9665633678222658,
'right_e1': -0.04985437554931641
}
wave_4 = {
'right_s0': 0.7857816576965333,
'right_s1': -0.1955825502319336,
'right_w0': -2.869694555657959,
'right_w1': 1.609912835046387,
'right_w2': 3.0591411827453614,
'right_e0': -1.9665633678222658,
'right_e1': -0.04985437554931641
}
limb.move_to_joint_positions(wave_1)
limb1.move_to_joint_positions(wave_2)
t_end = time.time() + 5
limb.move_to_joint_positions(wave_3)
limb.move_to_joint_positions(wave_4)
#wave_8={'right_s0': 0.6396699878173828, 'right_s1': -0.49202433715209964, 'right_w0': -1.7138400333068848, 'right_w1': 1.255563273449707, 'right_w2': -0.6066894009155274, 'right_e0': -1.7429856682434084, 'right_e1': 1.0492428577148438}
#limb.move_to_joint_positions(wave_8)
wave_7 = {
'right_s0': 0.9639937033411371,
'right_s1': -0.28695082759045815,
'right_w0': -2.959,
'right_w1': 1.5803892485976359,
'right_w2': -2.9352389229115348,
'right_e0': -0.14089182454059407,
'right_e1': 1.8593947307842924
}
limb.move_to_joint_positions(wave_7)
def __init__(self, ArmName):
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self.torcmd_now = np.array([[0.0], [0.0], [0.0], [0.0], [0.0], [0.0],
[0.0]])
self.name = ArmName
self.limb = baxter_interface.Limb(ArmName)
#self.limb.move_to_neutral()
t1 = self.limb.joint_angles()
t2 = [-0.528, -1.049, 1.131, 1.268, 0.618, 1.154, 2.416]
temp = 0
for key in t1:
t1[key] = t2[temp]
temp = temp + 1
self.limb.move_to_joint_positions(t1)
#self.limb.set_joint_position_speed(0.1)
self.actual_effort = self.limb.joint_efforts()
self.gravity_torques = self.limb.joint_efforts()
self.final_torques = self.gravity_torques.copy()
self.qnow = dict() # 得到关节角度的字典
self.qnow_value = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0]) # 得到角度的值
self.torcmd = dict() # 给baxter赋值
self.torController0 = PIDController()
self.torController1 = PIDController()
self.torController2 = PIDController()
self.torController3 = PIDController()
self.torController4 = PIDController()
self.torController5 = PIDController()
self.torController6 = PIDController()
self.torController = \
[self.torController0, self.torController1, self.torController2, self.torController3,
self.torController4, self.torController5, self.torController6
]
'''设置PID参数'''
# 最前端
self.torController[0].set_kp(17.7) # 130#80.0#*0.6
self.torController[0].set_ki(0.01)
self.torController[0].set_kd(5.1) # 10#15#0.01#*0.6#21.0
self.torController[1].set_kp(15) # 130#80.0#*0.6
self.torController[1].set_ki(6)
self.torController[1].set_kd(18) # 10#15#0.01#*0.6#21.0
self.torController[2].set_kp(15.7) # 130#80.0#*0.6
self.torController[2].set_ki(0.1)
self.torController[2].set_kd(1.2) # 10#15#0.01#*0.6#21.0
self.torController[3].set_kp(10.02) # 130#80.0#*0.6
self.torController[3].set_ki(1.2)
self.torController[3].set_kd(2.5) # 10#15#0.01#*0.6#21.0
self.torController[4].set_kp(10.3)
self.torController[4].set_ki(0.1) #0.1
self.torController[4].set_kd(2.1)
self.torController[5].set_kp(14.6) # 130#80.0#*0.6
self.torController[5].set_ki(1.5) #0.05
self.torController[5].set_kd(2.1) # 10#15#0.01#*0.6#21.0
self.torController[6].set_kp(22) # 130#80.0#*0.6
self.torController[6].set_ki(1.5)
self.torController[6].set_kd(4.5) # 10#15#0.01#*0.6#21.0
def map_keyboard(right_gripper):
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
def set_j(limb, joint_name, delta):
current_position = limb.joint_angle(joint_name)
joint_command = {joint_name: current_position + delta}
limb.set_joint_positions(joint_command)
bindings = {
# key: (function, args, description)
'9': (set_j, [left, lj[0], 0.1], "left_s0 increase"),
'6': (set_j, [left, lj[0], -0.1], "left_s0 decrease"),
'8': (set_j, [left, lj[1], 0.1], "left_s1 increase"),
'7': (set_j, [left, lj[1], -0.1], "left_s1 decrease"),
'o': (set_j, [left, lj[2], 0.1], "left_e0 increase"),
'y': (set_j, [left, lj[2], -0.1], "left_e0 decrease"),
'i': (set_j, [left, lj[3], 0.1], "left_e1 increase"),
'u': (set_j, [left, lj[3], -0.1], "left_e1 decrease"),
'l': (set_j, [left, lj[4], 0.1], "left_w0 increase"),
'h': (set_j, [left, lj[4], -0.1], "left_w0 decrease"),
'k': (set_j, [left, lj[5], 0.1], "left_w1 increase"),
'j': (set_j, [left, lj[5], -0.1], "left_w1 decrease"),
'.': (set_j, [left, lj[6], 0.1], "left_w2 increase"),
'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
',': (grip_left.close, [], "left: gripper close"),
'm': (grip_left.open, [], "left: gripper open"),
'/': (grip_left.calibrate, [], "left: gripper calibrate"),
'4': (set_j, [right, rj[0], 0.1], "right_s0 increase"),
'1': (set_j, [right, rj[0], -0.1], "right_s0 decrease"),
'3': (set_j, [right, rj[1], 0.1], "right_s1 increase"),
'2': (set_j, [right, rj[1], -0.1], "right_s1 decrease"),
'r': (set_j, [right, rj[2], 0.1], "right_e0 increase"),
'q': (set_j, [right, rj[2], -0.1], "right_e0 decrease"),
'e': (set_j, [right, rj[3], 0.1], "right_e1 increase"),
'w': (set_j, [right, rj[3], -0.1], "right_e1 decrease"),
'f': (set_j, [right, rj[4], 0.1], "right_w0 increase"),
'a': (set_j, [right, rj[4], -0.1], "right_w0 decrease"),
'd': (set_j, [right, rj[5], 0.1], "right_w1 increase"),
's': (set_j, [right, rj[5], -0.1], "right_w1 decrease"),
'v': (set_j, [right, rj[6], 0.1], "right_w2 increase"),
'z': (set_j, [right, rj[6], -0.1], "right_w2 decrease"),
'c': (grip_right.close, [], "right: gripper close"),
'x': (grip_right.open, [], "right: gripper open"),
'b': (grip_right.calibrate, [], "right: gripper calibrate"),
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
# c = baxter_external_devices.getch()
c = raw_input("Enter l' or 'r' and 7 joint angles: ")
arm = right
aj = rj
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
if c == 'get':
for i in range(7):
print("joint " + str(i) + ": " +
str(arm.joint_angle(aj[i])))
continue
angles = c.split()
angleList = []
if c[0] == 'l':
arm = left
aj = lj
else:
arm = right
aj = rj
print(arm)
print(aj)
#Close the left gripper
print('Closing...')
right_gripper.close(block=False)
rospy.sleep(1.0)
up_delta = arm.joint_angle(aj[1]) - 0.505446669
while abs(arm.joint_angle(aj[1]) - up_delta) > 0.01:
joint_command = {aj[1]: up_delta}
arm.set_joint_positions(joint_command)
for i in range(1, len(angles)):
angle_float = float(angles[i])
joint_command = {aj[i - 1]: angle_float}
while abs(arm.joint_angle(aj[i - 1]) - angle_float) > 0.01:
arm.set_joint_positions(joint_command)
#Open the left gripper
print('Opening...')
right_gripper.open(block=True)
rospy.sleep(1.0)
print('Done!')
for i in range(7):
print("joint " + str(i) + ": " + str(arm.joint_angle(aj[i])))
