def __init__(self):
self.left_arm = BaxterArm(
'left') #object of type Baxter from baxter_mechanism
self.right_arm = BaxterArm('right')
self.aruco_pose = rospy.Subscriber("/aruco_simple/pose", Pose,
self.aruco_pose_callback)
self.aruco_pose2 = rospy.Subscriber("/aruco_simple/pose2", Pose,
self.aruco_pose2_callback)
self.aruco_pose3 = rospy.Subscriber("/aruco_single/pose", PoseStamped,
self.aruco_pose3_callback)
self._jnt_calib_positions = None
self._total_joint_samples = 30
self._curr_indx = None
#to make sure that we get only readings when flags are enabled
self._capture_img = False
self._take_pose_reading = False
self._take_pose2_reading = False
self.pose_data_filled = False
self.pose2_data_filled = False
self.pose_pos = None
self.pose_ori = None
self.pose2_pos = None
self.pose2_ori = None
self.tf = TransformListener()
baxter = baxter_interface.RobotEnable(CHECK_VERSION)
self.br = tf.TransformBroadcaster()
baxter.enable()
def __init__(self,
extern_call=False,
trial_arm=None,
aux_arm=None,
tau=5.,
dt=0.05):
if extern_call:
if trial_arm is None or aux_arm is None:
print "Pass the arm handles, it can't be none"
raise ValueError
else:
self.left_arm = trial_arm
self.right_arm = aux_arm
else:
self.left_arm = BaxterArm(
'left') #object of type Baxter from baxter_mechanism
self.right_arm = BaxterArm('right')
self.start_pos = None
self.goal_pos = None
self.start_qt = None
self.goal_qt = None
self.tau = tau
self.dt = dt
self.timesteps = int(self.tau / self.dt)
baxter = baxter_interface.RobotEnable(CHECK_VERSION)
baxter.enable()
self.osc_pos_threshold = 0.01
def main(args):
rospy.init_node('get_test_images', anonymous=True)
limb = 'left'
arm = BaxterArm(limb)
ts = TestSetup(robot_interface=arm)
if args.operation == 'rec':
ts.record_test_data()
elif args.operation == 'load':
ts.load_test_data()
else:
print "Unknown argument \t", args.operation
class MinJerkController():
def __init__(self,
extern_call=False,
trial_arm=None,
aux_arm=None,
tau=5.,
dt=0.05):
if extern_call:
if trial_arm is None or aux_arm is None:
print "Pass the arm handles, it can't be none"
raise ValueError
else:
self.left_arm = trial_arm
self.right_arm = aux_arm
else:
self.left_arm = BaxterArm(
'left') #object of type Baxter from baxter_mechanism
self.right_arm = BaxterArm('right')
self.start_pos = None
self.goal_pos = None
self.start_qt = None
self.goal_qt = None
self.tau = tau
self.dt = dt
self.timesteps = int(self.tau / self.dt)
baxter = baxter_interface.RobotEnable(CHECK_VERSION)
baxter.enable()
self.osc_pos_threshold = 0.01
def configure(self, start_pos, start_ori, goal_pos, goal_ori):
self.start_pos = start_pos
self.goal_pos = goal_pos
self.start_qt = quaternion.as_float_array(start_ori)[0]
self.goal_qt = quaternion.as_float_array(goal_ori)[0]
def get_arm_handle(self, limb_idx=0):
if limb_idx == 0:
return self.left_arm
elif limb_idx == 1:
return self.right_arm
else:
print "Unknown limb index"
raise ValueError
def set_neutral(self):
self.left_arm.move_to_neutral()
self.right_arm.move_to_neutral()
def tuck_arms(self):
tuck_l = np.array([-1.0, -2.07, 3.0, 2.55, 0.0, 0.01, 0.0])
tuck_r = np.array([1.0, -2.07, -3.0, 2.55, -0.0, 0.01, 0.0])
self.left_arm.move_to_joint_position(tuck_l)
self.right_arm.move_to_joint_position(tuck_r)
def untuck_arms(self):
untuck_l = np.array([-0.08, -1.0, -1.19, 1.94, 0.67, 1.03, -0.50])
untuck_r = np.array([0.08, -1.0, 1.19, 1.94, -0.67, 1.03, 0.50])
self.left_arm.move_to_joint_position(untuck_l)
self.right_arm.move_to_joint_position(untuck_r)
def standard_shape_traj(self, curr_pos, no_set_points=16, shape='circle'):
if shape == 'circle':
r = 0.21
th = np.linspace(0., 2 * np.pi, no_set_points)
x = curr_pos[0] + r * np.cos(th)
y = curr_pos[1] + r * np.sin(th)
z = np.ones_like(x) * curr_pos[2]
elif shape == 'eight':
r = 0.21
th = np.linspace(0., 2 * np.pi, no_set_points)
x = curr_pos[0] + r * np.cos(th) / (1. + np.sin(th)**2)
y = curr_pos[1] + r * np.cos(th) * np.sin(th) / (1. +
np.sin(th)**2)
z = np.ones_like(x) * curr_pos[2]
else:
print "Enter a known shape"
raise ValueError
return np.vstack([x, y, z]).T
def get_min_jerk_trajectory(self):
final_q, final_w, final_al = min_jerk_step_qt(self.start_qt,
self.goal_qt, self.tau,
self.dt)
final_p, final_v, final_a = min_jerk_step_pos(self.start_pos,
self.goal_pos, self.tau,
self.dt)
min_jerk_traj = {}
#position trajectory
min_jerk_traj['pos_traj'] = final_p
#velocity trajectory
min_jerk_traj['vel_traj'] = final_v
#acceleration trajectory
min_jerk_traj['acc_traj'] = final_al
#orientation trajectory
min_jerk_traj['ori_traj'] = final_q
#angular velocity trajectory
min_jerk_traj['omg_traj'] = final_w[:, 1:4]
#angular acceleration trajectory
min_jerk_traj['alp_traj'] = final_a[:, 1:4]
return min_jerk_traj
def osc_position_cmd(self,
goal_pos,
goal_ori=None,
limb_idx=0,
orientation_ctrl=False):
arm = self.get_arm_handle(limb_idx)
jnt_start = arm.angles()
error = 100.
alpha = 0.1
t = 0
curr_pos, curr_ori = arm.get_ee_pose()
jac = arm.get_jacobian_from_joints()
arm_state = arm._update_state()
q = arm_state['position']
dq = arm_state['velocity']
delta_pos = goal_pos - curr_pos
if orientation_ctrl:
if goal_ori is None:
print "For orientation control, pass goal orientation!"
raise ValueError
delta_ori = quatdiff(
quaternion.as_float_array(goal_ori)[0],
quaternion.as_float_array(curr_ori)[0])
delta = np.hstack([delta_pos, delta_ori])
else:
jac = jac[0:3, :]
delta = delta_pos
jac_star = np.dot(jac.T, (np.linalg.inv(np.dot(jac, jac.T))))
null_q = alpha * np.dot(jac_star, delta) + np.dot(
(np.eye(len(jnt_start)) - np.dot(jac_star, jac)), (jnt_start - q))
u = q + null_q
if np.any(np.isnan(
u)) or np.linalg.norm(delta_pos) < self.osc_pos_threshold:
u = q
return u
def osc_torque_cmd(self,
goal_pos,
goal_ori=None,
limb_idx=0,
orientation_ctrl=False):
arm = self.get_arm_handle(limb_idx)
#secondary goal for the manipulator
if limb_idx == 0:
q_mean = np.array([-0.08, -1.0, -1.19, 1.94, 0.67, 1.03, -0.50])
elif limb_idx == 1:
q_mean = np.array([0.08, -1.0, 1.19, 1.94, -0.67, 1.03, 0.50])
else:
print "Unknown limb idex"
raise ValueError
#proportional gain
kp = 10.
#derivative gain
kd = np.sqrt(kp)
#null space control gain
alpha = 3.25
jnt_start = arm.angles()
#to fix the nan issues that happen
u_old = np.zeros_like(jnt_start)
# calculate position of the end-effector
ee_xyz, ee_ori = arm.get_ee_pose()
# calculate the Jacobian for the end effector
jac_ee = arm.get_jacobian_from_joints()
arm_state = arm._state
q = arm_state['position']
dq = arm_state['velocity']
h = arm_state['gravity_comp']
# calculate the inertia matrix in joint space
Mq = arm.get_arm_inertia()
# convert the mass compensation into end effector space
Mx_inv = np.dot(jac_ee, np.dot(np.linalg.inv(Mq), jac_ee.T))
svd_u, svd_s, svd_v = np.linalg.svd(Mx_inv)
# cut off any singular values that could cause control problems
singularity_thresh = .00025
for i in range(len(svd_s)):
svd_s[i] = 0 if svd_s[i] < singularity_thresh else \
1./float(svd_s[i])
# numpy returns U,S,V.T, so have to transpose both here
# convert the mass compensation into end effector space
Mx = np.dot(svd_v.T, np.dot(np.diag(svd_s), svd_u.T))
x_des = goal_pos - ee_xyz
if orientation_ctrl:
if goal_ori is None:
print "For orientation control, pass goal orientation!"
raise ValueError
else:
if type(goal_ori) is np.quaternion:
omg_des = quatdiff(
quaternion.as_float_array(goal_ori)[0],
quaternion.as_float_array(ee_ori)[0])
elif len(goal_ori) == 3:
omg_des = goal_ori
else:
print "Wrong dimension"
raise ValueError
else:
omg_des = np.zeros(3)
#print "x_des \n", x_des
#print "omg_des \n", omg_des
#print "eig values of Mx \n", np.linalg.eig(Mx)
#print "h: ", h
a_g = -np.dot(np.dot(jac_ee, np.linalg.inv(Mq)), h)
#print "a_g: ", a_g
# calculate desired force in (x,y,z) space
Fx = np.dot(Mx, np.hstack([x_des, omg_des]) + 0. * a_g)
#print "Fx \n", Fx
#print "F_x: ", Fx
# transform into joint space, add vel and gravity compensation
u = kp * np.dot(jac_ee.T, Fx) - np.dot(Mq, kd * dq)
#print "u \n", u
#print "h \n", h
#print "u: ", u
# calculate our secondary control signa
# calculated desired joint angle acceleration
q_des = (kp * (q_mean - q) - kd * dq).reshape(-1, )
u_null = np.dot(Mq, q_des)
# calculate the null space filter
Jdyn_inv = np.dot(Mx, np.dot(jac_ee, np.linalg.inv(Mq)))
null_filter = np.eye(len(q)) - np.dot(jac_ee.T, Jdyn_inv)
u_null_filtered = np.dot(null_filter, u_null)
u += alpha * u_null_filtered
if np.any(np.isnan(u)):
u = u_old
else:
u_old = u
return u
def relative_jac(self, rel_pos):
#left_arm is the master arm
jac_left = self.left_arm.get_jacobian_from_joints()
jac_right = self.right_arm.get_jacobian_from_joints()
def make_skew(v):
return np.array([[0., -v[2], v[1]], [v[2], 0., -v[0]],
[-v[1], v[0], 0.]])
tmp1 = np.vstack([
np.hstack([np.eye(3), -make_skew(rel_pos)]),
np.hstack([np.zeros((3, 3)), np.eye(3)])
])
pos_ee_l, rot_ee_l = self.left_arm.get_cartesian_pos_from_joints()
pos_ee_r, rot_ee_r = self.right_arm.get_cartesian_pos_from_joints()
tmp2 = np.vstack([
np.hstack([-rot_ee_l, np.zeros((3, 3))]),
np.hstack([np.zeros((3, 3)), -rot_ee_l])
])
tmp3 = np.vstack([
np.hstack([rot_ee_r, np.zeros((3, 3))]),
np.hstack([np.zeros((3, 3)), rot_ee_r])
])
jac_rel = np.hstack(
[np.dot(np.dot(tmp1, tmp2), jac_left),
np.dot(tmp3, jac_right)])
jac_master_rel = np.vstack(
[np.hstack([jac_left, np.zeros_like(jac_left)]), jac_rel])
return jac_master_rel
def osc_torque_cmd_2(self,
arm_data,
goal_pos,
goal_ori=None,
orientation_ctrl=False):
#proportional gain
kp = 10.
#derivative gain
kd = np.sqrt(kp)
#null space control gain
alpha = 0.0
jnt_start = arm_data['jnt_start']
ee_xyz = arm_data['ee_point']
jac_ee = arm_data['jacobian']
q = arm_data['position']
dq = arm_data['velocity']
ee_ori = arm_data['ee_ori']
#to fix the nan issues that happen
u_old = np.zeros_like(jnt_start)
# calculate the inertia matrix in joint space
Mq = arm_data['inertia']
# convert the mass compensation into end effector space
Mx_inv = np.dot(jac_ee, np.dot(np.linalg.inv(Mq), jac_ee.T))
svd_u, svd_s, svd_v = np.linalg.svd(Mx_inv)
# cut off any singular values that could cause control problems
singularity_thresh = .00025
for i in range(len(svd_s)):
svd_s[i] = 0 if svd_s[i] < singularity_thresh else \
1./float(svd_s[i])
# numpy returns U,S,V.T, so have to transpose both here
# convert the mass compensation into end effector space
Mx = np.dot(svd_v.T, np.dot(np.diag(svd_s), svd_u.T))
x_des = goal_pos #- ee_xyz
if orientation_ctrl:
if goal_ori is None:
print "For orientation control, pass goal orientation!"
raise ValueError
else:
if type(goal_ori) is np.quaternion:
omg_des = quatdiff(
quaternion.as_float_array(goal_ori)[0],
quaternion.as_float_array(ee_ori)[0])
elif len(goal_ori) == 3:
omg_des = goal_ori
else:
print "Wrong dimension"
raise ValueError
else:
omg_des = np.zeros(3)
# calculate desired force in (x,y,z) space
Fx = np.dot(Mx, np.hstack([x_des, omg_des]))
# transform into joint space, add vel and gravity compensation
u = (kp * np.dot(jac_ee.T, Fx) - np.dot(Mq, kd * dq))
# calculate our secondary control signal
# calculated desired joint angle acceleration
prop_val = ((jnt_start - q) + np.pi) % (np.pi * 2) - np.pi
q_des = (kp * prop_val - kd * dq).reshape(-1, )
u_null = np.dot(Mq, q_des)
# calculate the null space filter
Jdyn_inv = np.dot(Mx, np.dot(jac_ee, np.linalg.inv(Mq)))
null_filter = np.eye(len(q)) - np.dot(jac_ee.T, Jdyn_inv)
u_null_filtered = np.dot(null_filter, u_null)
#changing the rest q as the last updated q
jnt_start = q
u += alpha * u_null_filtered
if np.any(np.isnan(u)):
u = u_old
else:
u_old = u
return u
def plot_min_jerk_traj(self):
min_jerk_traj = self.get_min_jerk_trajectory()
import matplotlib.pyplot as plt
plt.figure(1)
plt.subplot(311)
plt.title('orientation')
plt.plot(min_jerk_traj['ori_traj'][:, 0])
plt.plot(min_jerk_traj['ori_traj'][:, 1])
plt.plot(min_jerk_traj['ori_traj'][:, 2])
plt.plot(min_jerk_traj['ori_traj'][:, 3])
#
plt.subplot(312)
plt.title('omega')
plt.plot(min_jerk_traj['omg_traj'][:, 0])
plt.plot(min_jerk_traj['omg_traj'][:, 1])
plt.plot(min_jerk_traj['omg_traj'][:, 2])
#
plt.subplot(313)
plt.title('alpha')
plt.plot(min_jerk_traj['alp_traj'][:, 0])
plt.plot(min_jerk_traj['alp_traj'][:, 1])
plt.plot(min_jerk_traj['alp_traj'][:, 2])
plt.figure(2)
plt.subplot(311)
plt.title('position')
plt.plot(min_jerk_traj['pos_traj'][:, 0])
plt.plot(min_jerk_traj['pos_traj'][:, 1])
plt.plot(min_jerk_traj['pos_traj'][:, 2])
#
plt.subplot(312)
plt.title('velocity')
plt.plot(min_jerk_traj['vel_traj'][:, 0])
plt.plot(min_jerk_traj['vel_traj'][:, 1])
plt.plot(min_jerk_traj['vel_traj'][:, 2])
#
plt.subplot(313)
plt.title('acceleration')
plt.plot(min_jerk_traj['acc_traj'][:, 0])
plt.plot(min_jerk_traj['acc_traj'][:, 1])
plt.plot(min_jerk_traj['acc_traj'][:, 2])
plt.show()
class Baxter_Eye_Hand_Calib():
def __init__(self):
self.left_arm = BaxterArm(
'left') #object of type Baxter from baxter_mechanism
self.right_arm = BaxterArm('right')
self.aruco_pose = rospy.Subscriber("/aruco_simple/pose", Pose,
self.aruco_pose_callback)
self.aruco_pose2 = rospy.Subscriber("/aruco_simple/pose2", Pose,
self.aruco_pose2_callback)
self.aruco_pose3 = rospy.Subscriber("/aruco_single/pose", PoseStamped,
self.aruco_pose3_callback)
self._jnt_calib_positions = None
self._total_joint_samples = 30
self._curr_indx = None
#to make sure that we get only readings when flags are enabled
self._capture_img = False
self._take_pose_reading = False
self._take_pose2_reading = False
self.pose_data_filled = False
self.pose2_data_filled = False
self.pose_pos = None
self.pose_ori = None
self.pose2_pos = None
self.pose2_ori = None
self.tf = TransformListener()
baxter = baxter_interface.RobotEnable(CHECK_VERSION)
self.br = tf.TransformBroadcaster()
baxter.enable()
def broadcast_frame(self, pt, rot, frame_name="marker"):
rot = np.append(rot, [[0, 0, 0]], 0)
rot = np.append(rot, [[0], [0], [0], [1]], 1)
quat = tuple(tf.transformations.quaternion_from_matrix(rot))
now = rospy.Time.now()
self.br.sendTransform((pt[0], pt[1], pt[2]),
tf.transformations.quaternion_from_matrix(rot),
now, frame_name, 'base')
print("should have done it!")
def aruco_pose_callback(self, data):
if self._take_pose_reading:
pos = np.array([data.position.x, data.position.y, data.position.z])
ori = np.quaternion(data.orientation.w, data.orientation.x,
data.orientation.y, data.orientation.z)
self._take_pose_reading = False
self.pose_pos = pos
self.pose_ori = ori
self.pose_data_filled = True
else:
return
def aruco_pose2_callback(self, data):
#if self._take_pose2_reading:
pos = np.array([data.position.x, data.position.y, data.position.z])
ori = np.quaternion(data.orientation.w, data.orientation.x,
data.orientation.y, data.orientation.z)
self._take_pose2_reading = False
self.pose2_pos = pos
self.pose2_ori = ori
#self.pose2_data_filled = True
#else:
# return
def aruco_pose3_callback(self, data):
#if self._take_pose2_reading:
pos = np.array(
[data.pose.position.x, data.pose.position.y, data.pose.position.z])
ori = np.quaternion(data.pose.orientation.w, data.pose.orientation.x,
data.pose.orientation.y, data.pose.orientation.z)
self._take_pose2_reading = False
self.pose2_pos = pos
self.pose2_ori = ori
#self.pose2_data_filled = True
#else:
# return
def save_calib_data(self):
calib_data = {}
calib_data['left_arm_calib_angle'] = self.left_arm.angles()
calib_data['right_arm_calib_angle'] = self.right_arm.angles()
got_common_time = False
max_attempts = 100
calibration_successful = False
counter = 0
while got_common_time is False and counter < max_attempts:
#some times the tf is busy that it fails to read the time and causes exception
# this simple hack waits till it reads it and saves it!!
try:
counter += 1
time = self.tf.getLatestCommonTime('openni_rgb_camera', 'base')
got_common_time = True
except tf.Exception:
print(
"Failed to find common time between openni_rgb_camera and base. Will try again. Attempt count %d/%d"
% (counter, max_attempts))
rospy.sleep(0.1)
if got_common_time:
translation, rot = self.tf.lookupTransform('openni_rgb_camera',
'base', time)
calib_data['openni_rgb_camera_pos'] = translation
calib_data['openni_rgb_camera_ori'] = rot
np.save('calib_data.npy', calib_data)
calibration_successful = True
else:
print("Failed to find transform from openni_rgb_camera to base")
return calib_data, calibration_successful
def load_calib_data(self):
# Load
try:
calib_data = np.load('calib_data.npy').item()
except Exception as e:
print "Caliberation file cannot be loaded"
raise e
print(calib_data['left_arm_calib_angle']) # displays "world"
print(calib_data['right_arm_calib_angle'])
# print(calib_data['openni_rgb_camera_pos'])
# print(calib_data['openni_rgb_camera_ori'])
return calib_data
def self_calibrate(self):
calib_data = self.load_calib_data()
self.left_arm.move_to_joint_position(
calib_data['left_arm_calib_angle'])
self.right_arm.move_to_joint_position(
calib_data['right_arm_calib_angle'])
calib_data, calib_success = self.save_calib_data()
if calib_success:
print "the openni_rgb_params \n"
print "postion \t", np.around(calib_data['openni_rgb_camera_pos'],
3)
print "orientation \t", np.around(
calib_data['openni_rgb_camera_ori'], 3)
else:
print "Calibration has failed!"
def get_box_transform(self):
flag = True
while True:
#some times the tf is busy that it fails to read the time and causes exception
# this simple hack waits till it reads it and saves it!!
try:
time = self.tf.getLatestCommonTime('box', 'base')
flag = False
except tf.Exception:
pass
if not flag:
translation, rot = self.tf.lookupTransform('box', 'base', time)
self.broadcast_frame(translation, rot, frame_name="marker_box")
flag = True
def map_keyboard():
# left = baxter_interface.Limb('left')
print "this"
left = BaxterArm('left')
right = BaxterArm('right')
kin = baxter_kinematics('left')
config = ALL_CONFIGS['position_baxter']
ctrlr = OSPositionController(left, config)
ctrlr.set_active(True)
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
leftdes = []
leftacc = []
lefterr = []
delta = 0.05 #was 0.05
gain = 1.
def IK(limb, movement):
current_pos, curr_ori = limb.ee_pose()
goal_pos = current_pos + movement
goal_ori = curr_ori
ctrlr.set_goal(goal_pos=goal_pos,
goal_ori=goal_ori,
orientation_ctrl=True)
lin_error, ang_error, success, time_elapsed = ctrlr.wait_until_goal_reached(
timeout=1.0)
#print (pos[1]-np.array(limb.endpoint_pose()['position'])[1])/delta 0.91 for x, 0.82 for z
# print (pos[2]-np.array(limb.endpoint_pose()['position'])[2])/delta
bindings = {
# key: (function, args, description)
's': (IK, [left, [delta, 0, 0]], "xinc"),
'd': (IK, [left, [-delta, 0, 0]], "xdec"),
'w': (IK, [left, [0, delta, 0]], "yinc"),
'e': (IK, [left, [0, -delta, 0]], "ydec"),
'x': (IK, [left, [0, 0, delta]], "zinc"),
'c': (IK, [left, [0, 0, -delta]], "zdec"),
#'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
'b': (grip_left.close, [], "left: gripper close"),
'n': (grip_left.open, [], "left: gripper open"),
'm': (grip_left.calibrate, [], "left: gripper calibrate") #comma here?
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "set_j(right, 's0', 0.1)"
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 map_keyboard():
left = baxter_interface.Limb('left')
# left = BaxterArm('left')
right = BaxterArm('right')
kin = baxter_kinematics('left')
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
lj = left.joint_names()
rj = right.joint_names()
leftdes = []
leftacc = []
lefterr = []
delta = 0.075 #was 0.05
gain = 1.
def IK(limb, movement):
# current_limb= limb.angles()
# joint_command = {joint_name: current_position + delta}
# current_pos, curr_ori = limb.ee_pose()
# leftacc = np.append(current_pos,quaternion.as_euler_angles(curr_ori))
# print leftacc.shape
joint_names = limb.joint_names()
# curr_q = limb.joint_angles()
def to_list(ls):
return [ls[n] for n in joint_names]
curr_q = np.asarray(to_list(limb.joint_angles())).reshape([7, 1])
# print curr_q.shape, 'shape'
# print 'ee pose', limb.endpoint_pose()
pos = np.array(limb.endpoint_pose()['position'])
#ori = limb.endpoint_pose()['orientation']
#pose = np.append(pos, quaternion.as_euler_angles(quaternion.quaternion(ori.w,ori.x,ori.y,ori.z))).reshape([6,1])
#print pose
# print pos, ori
# sldfjsl
# raw_input()
JT = kin.jacobian_transpose()
lefterr = np.append(movement, [0, 0, 0]).reshape([6, 1])
#lefterr = np.array(movement).reshape([3,1])
# leftdes = leftacc+lefterr
# print leftacc, lefterr, leftdes
#JT = np.linalg.pinv(limb.jacobian())
# JT = limb.jacobian().T
des_q = curr_q + gain * JT.dot(lefterr)
#des_q = JT.dot(lefterr + pose)
print des_q.shape, 'shape2'
q_dict = {}
for i in range(len(joint_names)):
q_dict[joint_names[i]] = des_q[i, :][0]
# print joint_names[i], q_dict[joint_names[i]]
# dsflasjkds
# joint_command = current_limb + JT.dot(lefterr)
# joint_command = JT.dot(leftdes)
# print joint_command
# print JT.dot(lefterr)
# limb.exec_position_cmd(joint_command)
# joint_command = {joint_name: curr_q + JT.dot(lefterr)}
# print q_dict
limb.move_to_joint_positions(q_dict)
#print (pos[1]-np.array(limb.endpoint_pose()['position'])[1])/delta 0.91 for x, 0.82 for z
print(pos[2] - np.array(limb.endpoint_pose()['position'])[2]) / delta
bindings = {
# key: (function, args, description)
# 's': (IK, [left, [delta,delta/100,delta/100] ], "xinc"),
#'d': (IK, [left, [-delta,-delta/100,-delta/100] ], "xdec"),
's': (IK, [left, [delta * 0.8, 0, 0]], "xinc"),
'd': (IK, [left, [-delta * 0.8, 0, 0]], "xdec"),
'w': (IK, [left, [0, delta * 0.5, 0]], "yinc"),
'e': (IK, [left, [0, -delta * 0.5, 0]], "ydec"),
#'w': (IK, [left, [-delta/20,delta,-delta/20] ], "yinc"),
# 'e': (IK, [left, [delta/20,-delta,delta/20] ], "ydec"),
'x': (IK, [left, [0, 0, delta]], "zinc"),
'c': (IK, [left, [0, 0, -delta]], "zdec"),
'k': (IK, [left, [0, 0, 0]], "nothing"),
'l': (IK, [left, [delta / 100, delta, delta / 100]], "allincy"),
';': (IK, [left, [-delta, -delta, -delta]], "alldec"),
#'n': (set_j, [left, lj[6], -0.1], "left_w2 decrease"),
'b': (grip_left.close, [], "left: gripper close"),
'n': (grip_left.open, [], "left: gripper open"),
'm': (grip_left.calibrate, [], "left: gripper calibrate") #comma here?
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = baxter_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
#expand binding to something like "set_j(right, 's0', 0.1)"
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]))