def set_joints( target_angles_right, target_angles_left):
right = Limb("right")
left = Limb("left")
reach_right = False
reach_left = False
while not reach_right or not reach_left:
right.set_joint_positions(target_angles_right)
left.set_joint_positions(target_angles_left)
current_angles_right = right.joint_angles()
current_angles_left = left.joint_angles()
for k, v in current_angles_right.iteritems():
if abs(target_angles_right[k] - v) > 0.01:
reach_right = False
break
reach_right = True
for k, v in current_angles_left.iteritems():
if abs(target_angles_left[k] - v) > 0.01:
reach_left = False
break
reach_left = True
class Baxter_Interactive(object):
def __init__(self, arm):
#Vector to record poses
self.recorded = []
self.arm = arm
#enable Baxter
self._en = RobotEnable()
self._en.enable()
#use DigitalIO to connect to gripper buttons
self._close_button = DigitalIO(self.arm + '_upper_button')
self._open_button = DigitalIO(self.arm + '_lower_button')
#set up gripper
self._gripper = Gripper(self.arm)
self._gripper.calibrate()
#set up navigator
self._navigator = Navigator(self.arm)
#set up limb
self._limb = Limb(self.arm)
self._limb.set_joint_position_speed(0.5)
self._limb.move_to_neutral()
print 'Ready to record...'
def recorder(self):
doneRecording = False
while not doneRecording:
if self._navigator.button0:
self.recorded.append(self._limb.joint_angles())
print 'Waypoint Added'
rospy.sleep(1)
if self._close_button.state:
self.recorded.append('CLOSE')
self._gripper.close()
print 'Gripper Closed'
rospy.sleep(1)
if self._open_button.state:
self.recorded.append('OPEN')
self._gripper.open()
print 'Gripper Opened'
rospy.sleep(1)
if self._navigator.button1:
print 'END RECORDING'
doneRecording = True
rospy.sleep(3)
while doneRecording:
for waypoint in self.recorded:
if waypoint == 'OPEN':
self._gripper.open()
rospy.sleep(1)
elif waypoint == 'CLOSE':
self._gripper.close()
rospy.sleep(1)
else:
self._limb.move_to_joint_positions(waypoint)
def set_joints( target_angles_right = None, target_angles_left = None,timeout= 40000):
right = Limb("right")
left = Limb("left")
if target_angles_right == None:
reach_right = True
else:
reach_right = False
if target_angles_left == None:
reach_left = True
else:
reach_left = False
time = 0
while not reach_right or not reach_left:
if target_angles_right: right.set_joint_positions(target_angles_right)
if target_angles_left: left.set_joint_positions(target_angles_left)
current_angles_right = right.joint_angles()
current_angles_left = left.joint_angles()
if reach_right == False:
for k, v in current_angles_right.iteritems():
if abs(target_angles_right[k] - v) > 0.01:
reach_right = False
break
reach_right = True
if reach_left == False:
for k, v in current_angles_left.iteritems():
if abs(target_angles_left[k] - v) > 0.01:
reach_left = False
break
reach_left = True
time+=1
if time > timeout:
print "Time out"
break
def set_joints( target_angles_right = None, target_angles_left = None,timeout= 40000):
right = Limb("right")
left = Limb("left")
if target_angles_right == None:
reach_right = True
else:
reach_right = False
if target_angles_left == None:
reach_left = True
else:
reach_left = False
time = 0
while not reach_right or not reach_left:
if target_angles_right: right.set_joint_positions(target_angles_right)
if target_angles_left: left.set_joint_positions(target_angles_left)
current_angles_right = right.joint_angles()
current_angles_left = left.joint_angles()
if reach_right == False:
for k, v in current_angles_right.iteritems():
if abs(target_angles_right[k] - v) > 0.01:
reach_right = False
break
reach_right = True
if reach_left == False:
for k, v in current_angles_left.iteritems():
if abs(target_angles_left[k] - v) > 0.01:
reach_left = False
break
reach_left = True
time+=1
if time > timeout:
print "Time out"
break
class TrajectoryWaypointsRecorder():
def __init__(self, limb_name, file_name):
self.limb = Limb(limb_name)
self.trajectory = []
self.file_name = file_name
self.file_header = self.limb.joint_names()
self.dt = rospy.Duration(secs=2)
self.previous_time = rospy.Time.now()
self.limb_nav = Navigator(limb_name)
self.limb_nav.button2_changed.connect(self.limb_nav_button_pressed)
rospy.on_shutdown(self.save_data)
rospy.loginfo('Press Ctrl + C to exit')
rospy.spin()
def save_data(self):
file_header = ','.join(x for x in self.file_header)
# src: http://wiki.ros.org/Packages#Client_Library_Support
full_path = os.path.join(
rospkg.RosPack().get_path('multiple_kinect_baxter_calibration'),
'files', self.file_name)
np.savetxt(full_path,
self.trajectory,
header=file_header,
delimiter=',',
fmt='%.4f',
comments='')
rospy.loginfo("Trajectory have been successfully saved to \n'%s'\n" %
full_path)
def limb_nav_button_pressed(self, state):
now = rospy.Time.now()
if (now - self.previous_time > self.dt) and state:
self.limb_nav.inner_led = state
self.record()
elif not state:
self.limb_nav.inner_led = state
self.previous_time = now
def record(self):
joint_angles_dict = self.limb.joint_angles()
joint_angles = [
joint_angles_dict[joint_name] for joint_name in self.file_header
]
self.trajectory.append(joint_angles)
rospy.loginfo('%d samples collected.' % len(self.trajectory))
class Baxter_Deli(object):
def __init__(self):
self.rightg = Gripper('right')
self.rightg.set_holding_force(100)
self.leftg = Gripper('left')
self.right = Limb('right')
self.left = Limb('left')
self.head = Head()
self.angles= list()
rospy.Subscriber("command", String, self.command)
rospy.spin()
def command(self, comm):
if comm.data == "left":
self.angles.append(self.left.joint_angles())
elif comm.data == "right":
self.angles.append(self.right.joint_angles())
elif comm.data == "done":
print self.angles
class Mimic(object):
IKSVC_LEFT_URI = 'ExternalTools/left/PositionKinematicsNode/IKService'
IKSVC_RIGHT_URI = 'ExternalTools/right/PositionKinematicsNode/IKService'
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber(
'/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay',
Image,
latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher(
'/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
############################################################################
def start(self):
self._rs.enable()
self._reset()
self._mimic_timer = rospy.Timer(rospy.Duration(1.0 / self._ik_rate),
self._mimic_callback)
rate = rospy.Rate(self._avg_window)
while not rospy.is_shutdown():
try:
(r_trans, r_rot) = self._tf.lookupTransform(
'/skeleton/user_1/right_hand', '/skeleton/user_1/torso',
rospy.Time(0))
(l_trans, l_rot) = self._tf.lookupTransform(
'/skeleton/user_1/left_hand', '/skeleton/user_1/torso',
rospy.Time(0))
self._l_trans_prev.append(l_trans)
if (len(self._l_trans_prev) > self._avg_window):
self._l_trans_prev.pop(0)
self._r_trans_prev.append(r_trans)
if (len(self._r_trans_prev) > self._avg_window):
self._r_trans_prev.pop(0)
except:
self._out_of_range = True
rate.sleep()
continue
rate.sleep()
def _reset(self):
if self._mimic_timer:
self._mimic_timer.shutdown()
self._left_arm.move_to_neutral()
self._left_arm_neutral = self._left_arm.joint_angles()
self._right_arm.move_to_neutral()
self._right_arm_neutral = self._right_arm.joint_angles()
print(self._left_arm_neutral)
print(self._right_arm_neutral)
def shutdown(self):
self._reset()
############################################################################
def _mimic_callback(self, event):
try:
l_trans = (sum(map(lambda x: x[0], self._l_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[1], self._l_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[2], self._l_trans_prev)) /
self._avg_window)
r_trans = (sum(map(lambda x: x[0], self._r_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[1], self._r_trans_prev)) /
self._avg_window,
sum(map(lambda x: x[2], self._r_trans_prev)) /
self._avg_window)
rh_x = clamp(0.65 + (l_trans[2] / 1.5), 0.5, 0.9)
rh_y = clamp(-l_trans[0], -0.8, 0)
rh_z = clamp(-l_trans[1], -0.1, 0.8)
lh_x = clamp(0.65 + (r_trans[2] / 1.5), 0.5, 0.9)
lh_y = clamp(-r_trans[0], 0, 0.8)
lh_z = clamp(-r_trans[1], -0.1, 0.8)
self.set_left_coords(lh_x, lh_y, lh_z)
self.set_right_coords(rh_x, rh_y, rh_z)
except:
return
def _head_camera_sub_callback(self, data):
orig_img = cvbr.imgmsg_to_cv2(data, 'rgb8')
img = cv2.cvtColor(orig_img, cv2.COLOR_RGB2GRAY)
img = cv2.equalizeHist(img)
img = cv2.GaussianBlur(img, (5, 5), 0)
img = cv2.Canny(img, 100, 200)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img = cv2.addWeighted(img, 0.75, orig_img, 0.25, 0)
img = cv2.resize(img, (1024, 768))
if self._out_of_range:
cv2.putText(img, 'Out of range!', (50, 768 - 50),
cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img = cvbr.cv2_to_imgmsg(img)
self._xdisplay_pub.publish(img)
############################################################################
def set_left_coords(self,
x,
y,
z,
er=math.pi * -1,
ep=math.pi * 0.5,
ey=math.pi * -1):
self._set_arm_coords(
self._left_iksvc, self._left_arm, x, y, z, er, ep, ey,
self._get_neutral_joint_state(self._left_arm_neutral),
self._l_ik_prev)
def set_right_coords(self,
x,
y,
z,
er=math.pi * -1,
ep=math.pi * 0.5,
ey=math.pi * -1):
self._set_arm_coords(
self._right_iksvc, self._right_arm, x, y, z, er, ep, ey,
self._get_neutral_joint_state(self._right_arm_neutral),
self._r_ik_prev)
############################################################################
def _set_arm_coords(self, iksvc, limb, x, y, z, er, ep, ey, njs, prev):
resp = self._get_ik(iksvc, x, y, z, er, ep, ey, njs)
positions = resp[0]
isValid = resp[1]
self._out_of_range = not isValid
prev.append(positions)
if (len(prev) > self._ik_smooth):
prev.pop(0)
smooth_positions = {}
for joint_name in positions:
smooth_positions[joint_name] = \
sum(map(lambda x: x[joint_name], prev)) / self._ik_smooth
limb.set_joint_positions(smooth_positions)
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs):
q = quaternion_from_euler(er, ep, ey)
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(position=Point(
x=x,
y=y,
z=z,
),
orientation=Quaternion(q[0], q[1], q[2], q[3])),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.seed_angles.append(njs)
iksvc.wait_for_service(1.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def _get_neutral_joint_state(self, njs):
js = JointState()
js.header = Header(stamp=rospy.Time.now(), frame_id='base')
for j in njs:
js.name.append(j)
js.position.append(njs[j])
return js
def listener():
global watch_timesteps
global firt_pack_sync
seq_len = 25
rospy.init_node('auto_grasping', anonymous=True, disable_signals=True)
model = load_model(pkg_dir + '/model/my_model25-94.h5')
zscore_data = np.loadtxt(pkg_dir + '/model/mean_std_zscore',
delimiter=',',
ndmin=2)
left_arm = Limb('left')
left_gripper = Gripper('left')
left_gripper.calibrate()
rate = rospy.Rate(50) # rate
rospy.Subscriber('/imu_giver', ImuPackage, watchCallback)
with firt_pack_sync:
firt_pack_sync.wait()
opened_timeout = 0
pre_res = 0
watch_buffer = []
bax_timesteps = []
# bax_timesteps and watch_buffer are two buffers to manage sequences
while not rospy.is_shutdown():
rate.sleep()
l_ang = list(left_arm.joint_angles().values())
l_vel = list(left_arm.joint_velocities().values())
l_eff = list(left_arm.joint_efforts().values())
bax_step = l_ang + l_vel + l_eff
bax_timesteps.append(bax_step)
if (watch_timesteps[1]):
watch_buffer.extend(watch_timesteps[0])
watch_timesteps[1] = 0
if (len(bax_timesteps) >= seq_len and len(watch_buffer) >= seq_len):
watch_buffer = watch_buffer[len(watch_buffer) - (seq_len):]
bax_timesteps = bax_timesteps[len(bax_timesteps) - (seq_len):]
sequence = []
for i in range(0, math.floor(seq_len * 0.3)):
step = watch_buffer.pop(0) + bax_timesteps.pop(0)
sequence.append(step)
for i in range(0, math.ceil(seq_len * 0.7)):
step = watch_buffer[i] + bax_timesteps[i]
sequence.append(step)
sequence = np.array(sequence)
sequence = sequence - zscore_data[0, :]
sequence = sequence / zscore_data[1, :]
seq = np.ndarray((1, seq_len, sequence.shape[1]))
seq[0] = sequence
res = model.predict(seq)
res = res[0][0]
rospy.loginfo(left_gripper.position())
if (left_gripper.position() > 94.0):
opened_timeout = opened_timeout + 1
if (res > 0.7 and pre_res > 0.7 and opened_timeout > 25):
left_gripper.command_position(0.0)
opened_timeout = 0
pre_res = res
def main():
"""
Main Script
"""
while not rospy.is_shutdown():
planner = PathPlanner("right_arm")
limb = Limb("right")
joint_angles = limb.joint_angles()
print(joint_angles)
camera_angle = {
'right_j6': 1.72249609375,
'right_j5': 0.31765625,
'right_j4': -0.069416015625,
'right_j3': 1.1111962890625,
'right_j2': 0.0664150390625,
'right_j1': -1.357775390625,
'right_j0': -0.0880478515625
}
limb.set_joint_positions(camera_angle)
limb.move_to_joint_positions(camera_angle,
timeout=15.0,
threshold=0.008726646,
test=None)
#drawing_angles = {'right_j6': -2.00561328125, 'right_j5': -1.9730205078125, 'right_j4': 1.5130146484375, 'right_j3': -1.0272568359375, 'right_j2': 1.24968359375, 'right_j1': -0.239498046875, 'right_j0': 0.4667275390625}
#print(joint_angles)
#drawing_angles = {'right_j6': -1.0133310546875, 'right_j5': -1.5432158203125, 'right_j4': 1.4599892578125, 'right_j3': -0.04361328125, 'right_j2': 1.6773486328125, 'right_j1': 0.019876953125, 'right_j0': 0.4214736328125}
drawing_angles = {
'right_j6': 1.9668515625,
'right_j5': 1.07505859375,
'right_j4': -0.2511611328125,
'right_j3': 0.782650390625,
'right_j2': 0.25496875,
'right_j1': -0.3268349609375,
'right_j0': 0.201146484375
}
raw_input("Press <Enter> to take picture: ")
waypoints_abstract = vision()
print(waypoints_abstract)
#ar coordinate :
x = 0.461067
y = -0.235305
#first get the solution of the maze
#solutionpoints = [(0,0),(-0.66,0.16), (-0.7, 0.4)]
# Make sure that you've looked at and understand path_planner.py before starting
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
r = rospy.Rate(10)
#find trans from
while not rospy.is_shutdown():
try:
trans = tfBuffer.lookup_transform('base', 'ar_marker_0',
rospy.Time()).transform
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
if tf2_ros.LookupException:
print("lookup")
elif tf2_ros.ConnectivityException:
print("connect")
elif tf2_ros.ExtrapolationException:
print("extra")
# print("not found")
pass
r.sleep()
mat = as_transformation_matrix(trans)
point_spaces = []
for point in waypoints_abstract:
# for point in solutionpoints:
point = np.array([point[0], point[1], 0, 1])
point_space = np.dot(mat, point)
point_spaces.append(point_space)
print(point_spaces)
length_of_points = len(point_spaces)
raw_input("Press <Enter> to move the right arm to drawing position: ")
limb.set_joint_positions(drawing_angles)
limb.move_to_joint_positions(drawing_angles,
timeout=15.0,
threshold=0.008726646,
test=None)
##
## Add the obstacle to the planning scene here
##
#add obstacle
size = [0.78, 1.0, 0.05]
name = "box"
pose = PoseStamped()
pose.header.frame_id = "base"
pose.pose.position.x = 0.77
pose.pose.position.y = 0.0
pose.pose.position.z = -0.18
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
planner.add_box_obstacle(size, name, pose)
#limb.set_joint_positions( drawing_angles)
#limb.move_to_joint_positions( drawing_angles, timeout=15.0, threshold=0.008726646, test=None)
#starting position
x, y, z = 0.67, 0.31, -0.107343
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
while not rospy.is_shutdown():
try:
waypoint = []
for point in point_spaces:
goal_1.pose.position.x = point[0]
goal_1.pose.position.y = point[1]
goal_1.pose.position.z = -0.113343 #set this value when put different marker
waypoint.append(copy.deepcopy(goal_1.pose))
plan, fraction = planner.plan_straight(waypoint, [])
print(fraction)
raw_input("Press <Enter> to move the right arm to draw: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
traceback.print_exc()
else:
break
raw_input("Press <Enter> to start again: ")
def main():
rospy.init_node('baxter_kinematics')
kin = baxter_kinematics('left')
pos = [0.582583, -0.180819, 0.216003]
rot = [0.03085, 0.9945, 0.0561, 0.0829]
fl = fcntl.fcntl(sys.stdin.fileno(), fcntl.F_GETFL)
fcntl.fcntl(sys.stdin.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
# Read initial positions:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
joints = left_arm.joint_names()
positions = left_arm.joint_angles()
velocities = left_arm.joint_velocities()
force = convert_dict_to_list('left', left_arm.joint_efforts())
# Initial states
q_previous = positions # Starting joint angles
q_dot_previous = velocities # Starting joint velocities
x_previous = kin.forward_position_kinematics(
) # Starting end effector position
x_dot_previous = np.zeros((6, 1))
# Set model parameters:
m = 1
K = 0.2
C = 15
B = 12
while True:
'''
Following code breaks loop upon user input (enter key)
'''
try:
stdin = sys.stdin.read()
if "\n" in stdin or "\r" in stdin:
return_to_init(joints, 'left')
break
except IOError:
pass
# Gather Jacobian information:
J = kin.jacobian()
J_T = kin.jacobian_transpose()
J_PI = kin.jacobian_pseudo_inverse()
J_T_PI = np.linalg.pinv(J_T)
# Extract sensor data:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
# Information is published at 100Hz by default (every 10ms=.01sec)
time_step = 0.01
joints = left_arm.joint_names()
positions = convert_dict_to_list('left', left_arm.joint_angles())
velocities = convert_dict_to_list('left', left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
force_mag = np.linalg.norm(force)
print(force_mag)
if (force_mag < 28): # Add deadzone
continue
else:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
joints = left_arm.joint_names()
positions = convert_dict_to_list('left', left_arm.joint_angles())
velocities = convert_dict_to_list('left',
left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
positions = np.reshape(positions, [7, 1]) # Converts to matrix
velocities = np.reshape(velocities, [7, 1]) # Converts to matrix
force = np.reshape(force, [7, 1]) # Converts to matrix
x = kin.forward_position_kinematics()
x = x[:-1]
x_ref = np.reshape(x, [6, 1]) # Reference position
x_ref_dot = J * velocities # Reference velocities
t_stop = 10
t_end = time.time() + t_stop # Loop timer (in seconds)
# Initial plot parameters
time_plot_cum = 0
time_vec = [time_plot_cum]
pos_vec = [x_ref[1][0]]
# For integral control
x_ctrl_cum = 0
time_cum = 0.00001 # Prevent divide by zero
x_ctrl_prev = 0 # Initial for derivative ctrl
while time.time() < t_end:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm = Limb('right')
left_arm = Limb('left')
J = kin.jacobian()
J_T = kin.jacobian_transpose()
J_PI = kin.jacobian_pseudo_inverse()
J_T_PI = np.linalg.pinv(J_T)
joints = left_arm.joint_names()
positions = convert_dict_to_list('left',
left_arm.joint_angles())
velocities = convert_dict_to_list('left',
left_arm.joint_velocities())
force = convert_dict_to_list('left', left_arm.joint_efforts())
positions = np.reshape(positions, [7, 1]) # Converts to matrix
velocities = np.reshape(velocities,
[7, 1]) # Converts to matrix
force = np.reshape(force, [7, 1]) # Converts to matrix
x = kin.forward_position_kinematics()
x = x[:-1]
x_current = np.reshape(x, [6, 1])
x_dot_current = J * velocities
# Only interested in y-axis:
x_dot_current[0] = 0
#x_dot_current[1]=0
x_dot_current[2] = 0
x_dot_current[3] = 0
x_dot_current[4] = 0
x_dot_current[5] = 0
x_ddot = derivative(x_dot_previous, x_dot_current, time_step)
# Model goes here
f = J_T_PI * force # spacial force
# Only interested in y-axis:
f[0] = [0]
#f[1]=[0]
f[2] = [0]
f[3] = [0]
f[4] = [0]
f[5] = [0]
x_des = ((B * f - m * x_ddot - C *
(x_dot_current - x_ref_dot)) /
K) + x_ref # Spring with damper
# Control robot
Kp = 0.0004
Ki = 0.00000022
Kd = 0.0000005
x_ctrl = x_current - x_des
# Only interested in y-axis:
x_ctrl[0] = 0
#x_ctrl[1]=0
x_ctrl[2] = 0
x_ctrl[3] = 0
x_ctrl[4] = 0
x_ctrl[5] = 0
q_dot_ctrl = J_T * np.linalg.inv(J * J_T + np.identity(6)) * (
-Kp * x_ctrl - Ki * (x_ctrl_cum) - Kd *
(x_ctrl_prev - x_ctrl) / time_step)
q_dot_ctrl_list = convert_mat_to_list(q_dot_ctrl)
q_dot_ctrl_dict = convert_list_to_dict(joints, q_dot_ctrl_list)
left_arm.set_joint_velocities(
q_dot_ctrl_dict) # Velocity control function
# Update plot info
time_cum += .05
time_vec.append(time_cum)
pos_vec.append(x_current[1][0])
# Update integral control parameters
x_ctrl_cum += x_ctrl * time_step
# Update derivative control parameters
x_ctrl_prev = x_ctrl
# Update values before next loop
x_previous = x_current # Updates previous position for next loop iteration
x_dot_previous = x_dot_current # Updates previous velocity for next loop iteration
print(time_vec)
print(pos_vec)
plt.plot(time_vec, pos_vec)
plt.title('Position over time')
plt.xlabel('Time (sec)')
plt.ylabel('Position')
plt.show()
stop_joints(q_dot_ctrl_dict)
left_arm.set_joint_velocities(q_dot_ctrl_dict)
time.sleep(1)
break
class Baxter(object):
def __init__(self, calibrate_grippers=True):
self._baxter_state = RobotEnable()
self._left = Limb(LEFT)
self._right = Limb(RIGHT)
self._limbs = {LEFT: self._left, RIGHT: self._right}
self._head = Head()
self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT)
if calibrate_grippers:
self.calibrate()
self._left_ikservice = IKService(LEFT)
self._right_ikservice = IKService(RIGHT)
def calibrate(self):
self._left_gripper.calibrate()
self._left_gripper_max = self._left_gripper.position()
self._right_gripper.calibrate()
self._right_gripper_max = self._right_gripper.position()
@property
def left_gripper_max(self):
return self._left_gripper_max
@property
def right_gripper_max(self):
return self._right_gripper_max
@property
def left_gripper(self):
return self._left_gripper.position()
@left_gripper.setter
def left_gripper(self, position):
self._left_gripper.command_position(position)
@property
def right_gripper(self):
return self._right_gripper.position()
@right_gripper.setter
def right_gripper(self, position):
self._right_gripper.command_position(position)
def set_left_joints(self, angles):
joints = self._left.joint_angles()
for joint, angle in angles.iteritems():
if angle:
joints[joint] = angle
self.enable_check()
self._left.set_joint_positions(joints)
def set_right_joints(self, angles):
joints = self._right.joint_angles()
for joint, angle in angles.iteritems():
if angle:
joints[joint] = angle
self.enable_check()
self._right.set_joint_positions(joints)
def reset_limb(self, side):
angles = {joint: 0.0 for joint in self._limbs[side].joint_angles()}
self.enable_check()
self._limbs[side].move_to_joint_positions(angles)
def enable_check(self):
# Sometimes robot is disabled due to another program resetting state
if not self._baxter_state.state().enabled:
self._baxter_state.enable()
@property
def joints(self):
joints = {
limb: joint.joint_angles()
for limb, joint in self._limbs.iteritems()
}
return joints
@property
def enabled(self):
return self._baxter_state.state().enabled
@property
def left_s0(self):
return self._left.joint_angle('left_s0')
@left_s0.setter
def left_s0(self, angle):
self.set_left_joints({'left_s0': angle})
@property
def left_s1(self):
return self._left.joint_angle('left_s1')
@left_s1.setter
def left_s1(self, angle):
self.set_left_joints({'left_s1': angle})
@property
def left_e0(self):
return self._left.joint_angle('left_e0')
@left_e0.setter
def left_e0(self, angle):
self.set_left_joints({'left_e0': angle})
@property
def left_e1(self):
return self._left.joint_angle('left_e1')
@left_e1.setter
def left_e1(self, angle):
self.set_left_joints({'left_e1': angle})
@property
def left_w0(self):
return self._left.joint_angle('left_w0')
@left_w0.setter
def left_w0(self, angle):
self.set_left_joints({'left_w0': angle})
@property
def left_w1(self):
return self._left.joint_angle('left_w1')
@left_w1.setter
def left_w1(self, angle):
self.set_left_joints({'left_w1': angle})
@property
def left_w2(self):
return self._left.joint_angle('left_w2')
@left_w2.setter
def left_w2(self, angle):
self.set_left_joints({'left_w2': angle})
@property
def right_s0(self):
return self._right.joint_angle('right_s0')
@right_s0.setter
def right_s0(self, angle):
self.set_right_joints({'right_s0': angle})
@property
def right_s1(self):
return self._right.joint_angle('right_s1')
@right_s1.setter
def right_s1(self, angle):
self.set_right_joints({'right_s1': angle})
@property
def right_e0(self):
return self._right.joint_angle('right_e0')
@right_e0.setter
def right_e0(self, angle):
self.set_right_joints({'right_e0': angle})
@property
def right_e1(self):
return self._right.joint_angle('right_e1')
@right_e1.setter
def right_e1(self, angle):
self.set_right_joints({'right_e1': angle})
@property
def right_w0(self):
return self._right.joint_angle('right_w0')
@right_w0.setter
def right_w0(self, angle):
self.set_right_joints({'right_w0': angle})
@property
def right_w1(self):
return self._right.joint_angle('right_w1')
@right_w1.setter
def right_w1(self, angle):
self.set_right_joints({'right_w1': angle})
@property
def right_w2(self):
return self._right.joint_angle('right_w2')
@right_w2.setter
def right_w2(self, angle):
self.set_right_joints({'right_w2': angle})
@property
def left_position(self):
return self._left.endpoint_pose()['position']
@property
def left_position_x(self):
return self.left_position.x
@left_position_x.setter
def left_position_x(self, point):
self.set_left_pose(position={'x': point})
@property
def left_position_y(self):
return self.left_position.y
@left_position_y.setter
def left_position_y(self, point):
self.set_left_pose(position={'y': point})
@property
def left_position_z(self):
return self.left_position.z
@left_position_z.setter
def left_position_z(self, point):
self.set_left_pose(position={'z': point})
@property
def left_orientation(self):
return self._left.endpoint_pose()['orientation']
@property
def left_orientation_x(self):
return self.left_orientation.x
@left_orientation_x.setter
def left_orientation_x(self, point):
self.set_left_pose(orientation={'x': point})
@property
def left_orientation_y(self):
return self.left_orientation.y
@left_orientation_y.setter
def left_orientation_y(self, point):
self.set_left_pose(orientation={'y': point})
@property
def left_orientation_z(self):
return self.left_orientation.z
@left_orientation_z.setter
def left_orientation_z(self, point):
self.set_left_pose(orientation={'z': point})
@property
def left_orientation_w(self):
return self.left_orientation.w
@left_orientation_w.setter
def left_orientation_w(self, point):
self.set_left_pose(orientation={'w': point})
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
def set_left_pose(self, position={}, orientation={}):
pos = {
'x': self.left_position_x,
'y': self.left_position_y,
'z': self.left_position_z,
}
for key, value in position.iteritems():
pos[key] = value
orient = {
'x': self.left_orientation_x,
'y': self.left_orientation_y,
'z': self.left_orientation_z,
'w': self.left_orientation_w,
}
for key, value in orientation.iteritems():
orient[key] = value
pos = self._left_ikservice.solve_position(
Pose(position=Point(**pos), orientation=Quaternion(**orient)))
if pos:
self.set_left_joints(pos)
else:
print 'nothing'
#print self.joints
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_position_x(self):
return self.right_position.x
@right_position_x.setter
def right_position_x(self, point):
self.set_right_pose(position={'x': point})
@property
def right_position_y(self):
return self.right_position.y
@right_position_y.setter
def right_position_y(self, point):
self.set_right_pose(position={'y': point})
@property
def right_position_z(self):
return self.right_position.z
@right_position_z.setter
def right_position_z(self, point):
self.set_right_pose(position={'z': point})
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
@property
def right_orientation_x(self):
return self.right_orientation.x
@right_orientation_x.setter
def right_orientation_x(self, point):
self.set_right_pose(orientation={'x': point})
@property
def right_orientation_y(self):
return self.right_orientation.y
@right_orientation_y.setter
def right_orientation_y(self, point):
self.set_right_pose(orientation={'y': point})
@property
def right_orientation_z(self):
return self.right_orientation.z
@right_orientation_z.setter
def right_orientation_z(self, point):
self.set_right_pose(orientation={'z': point})
@property
def right_orientation_w(self):
return self.right_orientation.w
@right_orientation_w.setter
def right_orientation_w(self, point):
self.set_right_pose(orientation={'w': point})
@property
def right_position(self):
return self._right.endpoint_pose()['position']
@property
def right_orientation(self):
return self._right.endpoint_pose()['orientation']
def set_right_pose(self, position={}, orientation={}):
pos = {
'x': self.right_position_x,
'y': self.right_position_y,
'z': self.right_position_z,
}
for key, value in position.iteritems():
pos[key] = value
orient = {
'x': self.right_orientation_x,
'y': self.right_orientation_y,
'z': self.right_orientation_z,
'w': self.right_orientation_w,
}
for key, value in orientation.iteritems():
orient[key] = value
pos = self._right_ikservice.solve_position(
Pose(position=Point(**pos), orientation=Quaternion(**orient)))
if pos:
self.set_right_joints(pos)
@property
def head_position(self):
return self._head.pan()
@head_position.setter
def head_position(self, position):
self._head.set_pan(position)
class Mimic(object):
IKSVC_LEFT_URI = 'ExternalTools/left/PositionKinematicsNode/IKService'
IKSVC_RIGHT_URI = 'ExternalTools/right/PositionKinematicsNode/IKService'
def __init__(self):
self._tf = tf.TransformListener()
self._rs = RobotEnable()
self._left_arm = Limb('left')
self._left_arm.set_joint_position_speed(0.5)
self._right_arm = Limb('right')
self._right_arm.set_joint_position_speed(0.5)
self._left_arm_neutral = None
self._right_arm_neutral = None
self._left_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_LEFT_URI,
SolvePositionIK)
self._right_iksvc = rospy.ServiceProxy(
Mimic.IKSVC_RIGHT_URI,
SolvePositionIK)
self._left_camera = CameraController('left_hand_camera')
self._right_camera = CameraController('right_hand_camera')
self._head_camera = CameraController('head_camera')
self._left_camera.close()
self._right_camera.close()
self._head_camera.close()
self._head_camera.resolution = CameraController.MODES[0]
self._head_camera.open()
self._head_camera_sub = rospy.Subscriber('/cameras/head_camera/image', Image,
self._head_camera_sub_callback)
self._xdisplay_pub = rospy.Publisher('/robot/xdisplay', Image, latch=True)
self._out_of_range = False
self._ik_smooth = 4
self._ik_rate = 200
self._avg_window = 1000
self._r_trans_prev = []
self._l_trans_prev = []
self._r_ik_prev = []
self._l_ik_prev = []
self._joint_update_pub = rospy.Publisher('/robot/joint_state_publish_rate', UInt16)
self._joint_update_pub.publish(self._ik_rate)
self._mimic_timer = None
############################################################################
def start(self):
self._rs.enable()
self._reset()
self._mimic_timer = rospy.Timer(rospy.Duration(1.0 / self._ik_rate), self._mimic_callback)
rate = rospy.Rate(self._avg_window)
while not rospy.is_shutdown():
try:
(r_trans,r_rot) = self._tf.lookupTransform(
'/skeleton/user_1/right_hand',
'/skeleton/user_1/torso',
rospy.Time(0))
(l_trans,l_rot) = self._tf.lookupTransform(
'/skeleton/user_1/left_hand',
'/skeleton/user_1/torso',
rospy.Time(0))
self._l_trans_prev.append(l_trans)
if (len(self._l_trans_prev) > self._avg_window):
self._l_trans_prev.pop(0)
self._r_trans_prev.append(r_trans)
if (len(self._r_trans_prev) > self._avg_window):
self._r_trans_prev.pop(0)
except:
self._out_of_range = True
rate.sleep()
continue
rate.sleep()
def _reset(self):
if self._mimic_timer:
self._mimic_timer.shutdown()
self._left_arm.move_to_neutral()
self._left_arm_neutral = self._left_arm.joint_angles()
self._right_arm.move_to_neutral()
self._right_arm_neutral = self._right_arm.joint_angles()
print(self._left_arm_neutral)
print(self._right_arm_neutral)
def shutdown(self):
self._reset()
############################################################################
def _mimic_callback(self, event):
try:
l_trans = (
sum(map(lambda x: x[0], self._l_trans_prev)) / self._avg_window,
sum(map(lambda x: x[1], self._l_trans_prev)) / self._avg_window,
sum(map(lambda x: x[2], self._l_trans_prev)) / self._avg_window)
r_trans = (
sum(map(lambda x: x[0], self._r_trans_prev)) / self._avg_window,
sum(map(lambda x: x[1], self._r_trans_prev)) / self._avg_window,
sum(map(lambda x: x[2], self._r_trans_prev)) / self._avg_window)
rh_x = clamp(0.65 + (l_trans[2] / 1.5), 0.5, 0.9)
rh_y = clamp(-l_trans[0], -0.8, 0)
rh_z = clamp(-l_trans[1], -0.1, 0.8)
lh_x = clamp(0.65 + (r_trans[2] / 1.5), 0.5, 0.9)
lh_y = clamp(-r_trans[0], 0, 0.8)
lh_z = clamp(-r_trans[1], -0.1, 0.8)
self.set_left_coords(lh_x, lh_y, lh_z)
self.set_right_coords(rh_x, rh_y, rh_z)
except:
return
def _head_camera_sub_callback(self, data):
orig_img = cvbr.imgmsg_to_cv2(data, 'rgb8')
img = cv2.cvtColor(orig_img, cv2.COLOR_RGB2GRAY)
img = cv2.equalizeHist(img)
img = cv2.GaussianBlur(img, (5, 5), 0)
img = cv2.Canny(img, 100, 200)
img = cv2.cvtColor(img, cv2.COLOR_GRAY2RGB)
img = cv2.addWeighted(img, 0.75, orig_img, 0.25, 0)
img = cv2.resize(img, (1024, 768))
if self._out_of_range:
cv2.putText(img, 'Out of range!', (50, 768-50), cv2.FONT_HERSHEY_SIMPLEX, 2, (255, 0, 0), 4)
img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
img = cvbr.cv2_to_imgmsg(img)
self._xdisplay_pub.publish(img)
############################################################################
def set_left_coords(self,
x, y, z,
er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1):
self._set_arm_coords(self._left_iksvc, self._left_arm,
x, y, z,
er, ep, ey,
self._get_neutral_joint_state(self._left_arm_neutral),
self._l_ik_prev)
def set_right_coords(self,
x, y, z,
er=math.pi * -1, ep=math.pi * 0.5, ey=math.pi * -1):
self._set_arm_coords(self._right_iksvc, self._right_arm,
x, y, z,
er, ep, ey,
self._get_neutral_joint_state(self._right_arm_neutral),
self._r_ik_prev)
############################################################################
def _set_arm_coords(self, iksvc, limb, x, y, z, er, ep, ey, njs, prev):
resp = self._get_ik(iksvc, x, y, z, er, ep, ey, njs)
positions = resp[0]
isValid = resp[1]
self._out_of_range = not isValid
prev.append(positions)
if (len(prev) > self._ik_smooth):
prev.pop(0)
smooth_positions = {}
for joint_name in positions:
smooth_positions[joint_name] = \
sum(map(lambda x: x[joint_name], prev)) / self._ik_smooth
limb.set_joint_positions(smooth_positions)
def _get_ik(self, iksvc, x, y, z, er, ep, ey, njs):
q = quaternion_from_euler(er, ep, ey)
pose = PoseStamped(
header=Header(stamp=rospy.Time.now(), frame_id='base'),
pose=Pose(
position=Point(
x=x,
y=y,
z=z,
),
orientation=Quaternion(q[0], q[1], q[2], q[3])
),
)
ikreq = SolvePositionIKRequest()
ikreq.pose_stamp.append(pose)
ikreq.seed_angles.append(njs)
iksvc.wait_for_service(1.0)
resp = iksvc(ikreq)
positions = dict(zip(resp.joints[0].name, resp.joints[0].position))
return (positions, resp.isValid[0])
def _get_neutral_joint_state(self, njs):
js = JointState()
js.header = Header(stamp=rospy.Time.now(), frame_id='base')
for j in njs:
js.name.append(j)
js.position.append(njs[j])
return js
def talker(origin_frame):
#Run this program as a new node in the ROS computation graph
#called /talker.
rospy.init_node('main_controller', anonymous=True)
#Create an instance of the rospy.Publisher object which we can
#use to publish messages to a topic.
pub = rospy.Publisher('gripper', String, queue_size=10)
#Create a tf buffer, which is primed with a tf listener
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
planner = PathPlanner("right_arm")
right = Limb('right')
# rj = right.joint_names()
r = rospy.Rate(10)
# Loop until the node is killed with Ctrl-C
while not rospy.is_shutdown():
try:
pub_string = raw_input('Type [open] or [close]: ')
# Publish our string to the 'chatter_talk' topic
pub.publish(pub_string)
while not rospy.is_shutdown():
try:
raw_input("Press <Enter> to move the right arm to goal pose 1: ")
plan = planner.plan_to_joint_default()
# print(plan)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
time.sleep(2)
except Exception as e:
print e
traceback.print_exc()
else:
break
trans_to_wrist = tfBuffer.lookup_transform(origin_frame, "right_hand", rospy.Time())
# trans_to_tip = tfBuffer.lookup_transform(origin_frame, "right_gripper", rospy.Time())
trans_to_base = tfBuffer.lookup_transform(origin_frame, "right_lower_shoulder", rospy.Time())
trans_to_cup = tfBuffer.lookup_transform(origin_frame, "cup_center", rospy.Time())
gun = [0,0,0]
base = [0,0,0]
goal = [0,0,0]
# gun[0] = (trans_to_wrist.transform.translation.x+trans_to_tip.transform.translation.x)/2
# gun[1] = (trans_to_wrist.transform.translation.y+trans_to_tip.transform.translation.y)/2
# gun[2] = (trans_to_wrist.transform.translation.z+trans_to_tip.transform.translation.z)/2
gun[0] = trans_to_wrist.transform.translation.x
gun[1] = trans_to_wrist.transform.translation.y
gun[2] = trans_to_wrist.transform.translation.z
base[0] = trans_to_base.transform.translation.x
base[1] = trans_to_base.transform.translation.y
base[2] = trans_to_base.transform.translation.z
goal[0] = trans_to_cup.transform.translation.x
goal[1] = trans_to_cup.transform.translation.y
# height of cup
goal[2] = 0.12065/2
theta1 = Projectile.calc_xy(goal, base, gun)
print("theta1 = "+str(theta1))
while not rospy.is_shutdown():
try:
raw_input("Press <Enter> to move the right arm to theta1: ")
goal_joints = right.joint_angles()
goal_joints["right_s0"] += np.radians(theta1)
plan = planner.plan_to_joint(goal_joints)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
time.sleep(2)
except Exception as e:
print e
traceback.print_exc()
else:
break
trans_to_wrist = tfBuffer.lookup_transform(origin_frame, "right_hand", rospy.Time())
# trans_to_tip = tfBuffer.lookup_transform(origin_frame, "right_gripper", rospy.Time())
# gun[0] = (trans_to_wrist.transform.translation.x+trans_to_tip.transform.translation.x)/2
# gun[1] = (trans_to_wrist.transform.translation.y+trans_to_tip.transform.translation.y)/2
# gun[2] = (trans_to_wrist.transform.translation.z+trans_to_tip.transform.translation.z)/2
gun[0] = trans_to_wrist.transform.translation.x
gun[1] = trans_to_wrist.transform.translation.y
gun[2] = trans_to_wrist.transform.translation.z
theta2 = gripper_sub.aim_z(gun,goal)
print("theta2 = "+str(np.degrees(theta2)))
while not rospy.is_shutdown():
try:
raw_input("Press <Enter> to move the right arm to theta2: ")
goal_joints = right.joint_angles()
goal_joints["right_w2"] = pi/2-theta2 + np.radians(OFFSET)
plan = planner.plan_to_joint(goal_joints)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
time.sleep(2)
except Exception as e:
print e
traceback.print_exc()
else:
break
# Construct a string that we want to publish
# (In Python, the "%" operator functions similarly
# to sprintf in C or MATLAB)
pub_string = raw_input('Type [open] or [close]: ')
# Publish our string to the 'chatter_talk' topic
pub.publish(pub_string)
# print(np.degrees(joint_angles))
# print(delta)
# if np.amax(error) >= .1:
# print("go")
# gripper_sub.default_state(JOINT_DEFAULT)
# aim_xy(end_coord, base_coord, cup_coord, joint_state)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
print('loading')
pass
r.sleep()
def listener():
rospy.init_node('record_data', anonymous=True, disable_signals=True)
global BAX_COLUMNS
global WATCH_COLUMNS
global NANOS_TO_MILLIS
global bax_file_name
global bax_row
global watch_rows
global command
global sequence_counter
resetNode()
rospy.loginfo("Commands :\ns to stop\nr to remove the last file\nn to start new sequence\nc TWICE to shutdown the node\n")
rate = rospy.Rate(120) # rate
watch_sub = rospy.Subscriber('/imu_giver', ImuPackage, watchCallback)
rospy.loginfo("START RECORDING SEQUENCE " + str(sequence_counter))
getkey_thread = Thread(target = getKey)
getkey_thread.start()
left_arm = Limb('left')
left_gripper = Gripper('left')
while not rospy.is_shutdown():
rate.sleep()
t = round(rospy.get_rostime().to_nsec()*NANOS_TO_MILLIS)
l_ang = list(left_arm.joint_angles().values())
l_vel = list(left_arm.joint_velocities().values())
l_eff = list(left_arm.joint_efforts().values())
l_grip_pos = str(left_gripper.position())
bax_row = l_ang + l_vel + l_eff
bax_row.append(l_grip_pos)
bax_row.append(str(t))
with open(bax_file_name + str(sequence_counter), 'a') as writeFile:
writer = csv.writer(writeFile)
writer.writerow(bax_row)
writeFile.close()
if (watch_rows[1]==1):
with open(watch_file_name + str(sequence_counter), 'a') as writeFile:
writer = csv.writer(writeFile)
writer.writerows(watch_rows[0])
writeFile.close()
watch_rows[1]=0
# s to stop
# r to remove the last file
# n to start new sequence
# c TWICE to shutdown the node
shutdown = False
if (command == 's') :
watch_sub.unregister()
rospy.loginfo("FINISH RECORDING SEQUENCE " + str(sequence_counter))
rospy.loginfo("NODE STOPPED!")
while True :
rospy.Rate(2).sleep()
if (command == 'r') :
os.remove(bax_file_name + str(sequence_counter))
os.remove(watch_file_name + str(sequence_counter))
sequence_counter = sequence_counter - 1
rospy.loginfo("FILE REMOVED!")
command = ''
if (command == 'n') :
rospy.loginfo("RESET NODE!")
sequence_counter = sequence_counter + 1
resetNode()
watch_sub = rospy.Subscriber('/imu_giver', ImuPackage, watchCallback)
rospy.loginfo("START RECORDING SEQUENCE " + str(sequence_counter))
break
if (command == 'c') :
rospy.loginfo("Enter 'c' to shutdown... ")
shutdown = True
break
if (shutdown) :
rospy.signal_shutdown("reason...")
getkey_thread.join()
class Baxter(object):
"""
Iinterface for controlling the real and simulated
Baxter robot.
"""
def __init__(self,
sim=False,
config=None,
time_step=1.0,
rate=100.0,
missed_cmds=20000.0):
"""
Initialize Baxter Interface
Args:
sim (bool): True specifies using the PyBullet simulator
False specifies using the real robot
arm (str): 'right' or 'left'
control (int): Specifies control type
TODO: change to str ['ee', 'position', 'velocity']
config (class): Specifies joint ranges, ik null space paramaters, etc
time_step (float):
TODO: probably get rid of this
rate (float):
missed_cmds (float):
"""
self.sim = sim
self.dof = {
'left': {
'ee': 6,
'position': 7,
'velocity': 7
},
'right': {
'ee': 6,
'position': 7,
'velocity': 7
},
'both': {
'ee': 12,
'position': 14,
'velocity': 14
}
}
if self.sim:
import pybullet as p
import baxter_pybullet_interface as pybullet_interface
self.baxter_path = "assets/baxter_robot/baxter_description/urdf/baxter.urdf"
self.time_step = time_step
else:
import rospy
from baxter_pykdl import baxter_kinematics
import baxter_interface
from baxter_interface import CHECK_VERSION, Limb, Gripper
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from std_msgs.msg import Header
from baxter_core_msgs.srv import SolvePositionIK, SolvePositionIKRequest
self.rate = rate
self.freq = 1 / rate
self.missed_cmds = missed_cmds
self.config = config
self.initial_setup()
def set_command_time_out(self):
"""
Set command timeout for sending ROS commands
"""
if self.sim:
pass
else:
self.left_arm.set_command_timeout(self.freq * self.missed_cmds)
self.right_arm.set_command_timeout(self.freq * self.missed_cmds)
return
def initial_setup(self):
if self.sim:
# load baxter
objects = [p.loadURDF(self.baxter_path, useFixedBase=True)]
self.baxter_id = objects[0]
print("baxter_id: ", self.baxter_id)
if self.config:
self.use_nullspace = True
self.use_orientation = True
self.ll = self.config.nullspace.ll
self.ul = self.config.nullspace.ul
self.jr = self.config.nullspace.jr
self.rp = self.config.nullspace.rp
else:
self.use_nullspace = False
self.create_arms()
else:
self.create_arms()
self.set_command_time_out()
self.control_rate = rospy.Rate(self.rate)
def reset(self, initial_pose=None):
if self.sim:
self._reset_sim(initial_pose)
else:
self._reset_real(initial_pose)
def _reset_real(self, initial_pose=None):
self.enable()
time.sleep(0.5)
self.set_initial_position('right')
self.set_initial_position('left', blocking=True)
self.calibrate_grippers()
def _reset_sim(self, control_type=None, initial_pose=None):
# set control type
if control_type == 'position' or control_type == 'ee' or control_type is None:
control_mode = p.POSITION_CONTROL
elif control_type == 'velocity':
control_mode = p.VELOCITY_CONTROL
elif control_mode == 'torque':
control_mode = p.TORQUE_CONTROL
else:
raise ValueError(
"control_type must be in ['position', 'ee', 'velocity', 'torque']."
)
# set initial position
if initial_pose:
for joint_index, joint_val in initial_pose:
p.resetJointState(0, joint_index, joint_val)
p.setJointMotorControl2(self.baxter_id,
jointIndex=joint_index,
controlMode=control_mode,
targetPosition=joint_val)
else:
num_joints = p.getNumJoints(self.baxter_id)
for joint_index in range(num_joints):
p.setJointMotorControl2(self.baxter_id,
joint_index,
control_mode,
maxForce=self.config.max_force)
return
def create_arms(self):
"""
Create arm interface objects for Baxter.
An arm consists of a Limb and its Gripper.
"""
# create arm objects
if self.sim:
self.left_arm = pybullet_interface.Limb(self.baxter_id, "left",
self.config)
# self.left_arm.gripper = pybullet_interface.Gripper("left")
self.right_arm = pybullet_interface.Limb(self.baxter_id, "right",
self.config)
# self.right_arm.gripper = pybullet_interface.Gripper("right")
else:
self.left_arm = Limb("left")
self.left_arm.gripper = Gripper("left")
self.left_arm.kin = baxter_kinematics("left")
self.right_arm = Limb("right")
self.right_arm.gripper = Gripper("right")
self.right_arm.kin = baxter_kinematics("right")
return
def calibrate_grippers(self):
"""
(Blocking) Calibrates gripper(s) if not
yet calibrated
"""
if not self.left_arm.gripper.calibrated():
self.left_arm.gripper.calibrate()
if not self.right_arm.gripper.calibrated():
self.right_arm.gripper.calibrate()
return
def enable(self):
self._rs = baxter_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
self._rs.enable()
def shutdown(self):
pass
def set_joint_velocities(self, arm, joint_velocities):
"""
Set joint velocites for a given arm
Args
arm (str): 'left' or 'right' or 'both'
joint_velocities (list or tuple or numpy array):
Array of len 7 or 14
"""
if arm == 'left':
action_dict = self.create_action_dict('left', 'velocity',
joint_velocities)
self.left_arm.set_joint_velocities(action_dict)
elif arm == 'right':
action_dict = self.create_action_dict('right', 'velocity',
joint_velocities)
self.right_arm.set_joint_velocities(action_dict)
elif arm == 'both':
l_velocities = joint_velocities[:7]
r_velocities = joint_velocities[7:]
l_action_dict = self.create_action_dict('left', 'velocity',
l_velocities)
r_action_dict = self.create_action_dict('right', 'velocity',
r_velocities)
self.left_arm.set_joint_velocities(l_action_dict)
self.right_arm.set_joint_velocities(r_action_dict)
return
def move_to_joint_positions(self, arm, joint_positions):
"""
Move specified arm to give joint positions
(This function is blocking.)
Args
arm (str): 'left' or 'right' or 'both'
joint_positionns (list or tuple or numpy array):
Array of len 7 or 14
"""
if arm == 'left':
action_dict = self.create_action_dict('left', 'position',
joint_positions)
self.left_arm.move_to_joint_positions(action_dict)
elif arm == 'right':
action_dict = self.create_action_dict('right', 'position',
joint_positions)
self.right_arm.move_to_joint_positions(action_dict)
elif arm == 'both':
l_joints = joint_positions[:7]
r_joints = joint_positions[7:]
l_action_dict = self.create_action_dict('left', 'position',
l_joints)
r_action_dict = self.create_action_dict('right', 'position',
r_joints)
self.left_arm.move_to_joint_positions(l_action_dict)
self.right_arm.move_to_joint_positions(r_action_dict)
return
def set_joint_positions(self, arm, joint_positions):
"""
Move specified arm to give joint positions
(This function is not blocking.)
Args
arm (str): 'left' or 'right' or 'both'
joint_positionns (list or tuple or numpy array):
Array of len 7 or 14
"""
if arm == 'left':
action_dict = self.create_action_dict('left', 'position',
joint_positions)
self.left_arm.set_joint_positions(action_dict)
elif arm == 'right':
action_dict = self.create_action_dict('right', 'position',
joint_positions)
self.right_arm.set_joint_positions(action_dict)
elif arm == 'both':
l_joints = joint_positions[:7]
r_joints = joint_positions[7:]
l_action_dict = self.create_action_dict('left', 'position',
l_joints)
r_action_dict = self.create_action_dict('right', 'position',
r_joints)
self.left_arm.set_joint_positions(l_action_dict)
self.right_arm.set_joint_positions(r_action_dict)
return
def move_to_ee_pose(self, arm, pose, blocking=True):
"""
Move end effector to specified pose
Args
arm (string): "left" or "right" or "both"
pose (list):
if arm == 'left' or arm == 'right':
pose = [X, Y, Z, r, p, w]
else:
pose = left_pose + right _pose
"""
if arm == "both":
left_pose = pose[:6]
right_pose = pose[6:]
left_joints = self.ik('left', left_pose)
right_joints = self.ik('right', right_pose)
if blocking:
self.left_arm.move_to_joint_positions(left_joints)
self.right_arm.move_to_joint_positions(right_joints)
else:
self.left_arm.set_joint_positions(left_joints)
self.right_arm.set_joint_positions(right_joints)
elif arm == "left":
joints = self.ik(arm, pose)
if blocking:
self.left_arm.move_to_joint_positions(joints)
else:
self.left_arm.set_joint_positions(joints)
elif arm == "right":
joints = self.ik(arm, pose)
if blocking:
self.right_arm.move_to_joint_positions(joints)
else:
self.right_arm.set_joint_positions(joints)
else:
raise ValueError("Arm must be 'right', 'left', or 'both'")
return
def get_ee_pose(self, arm, mode=None):
"""
Returns end effector pose for specified arm.
End effector pose is a list of values corresponding to the 3D cartesion coordinates
and roll (r), pitch (p), and yaw (w) Euler angles.
Args
arm (string): "right" or "left"
mode (string): "Quaternion" or "quaterion" or "quat"
Returns
pose (list): Euler angles [X,Y,Z,r,p,w] or Quaternion [X,Y,Z,x,y,z,w]
"""
if arm == 'left' or arm == 'right':
pos = self.get_ee_position(arm)
orn = self.get_ee_orientation(arm, mode)
return pos + orn
elif arm == 'both':
l_pos = self.get_ee_position('left')
l_orn = self.get_ee_orientation('left', mode)
r_pos = self.get_ee_position('right')
r_orn = self.get_ee_orientation('right', mode)
return l_pos + l_orn + r_pos + r_orn
def get_ee_position(self, arm):
"""
Returns end effector position for specified arm.
Returns the 3D cartesion coordinates of the end effector.
Args
arm (string): "left" or "right" or "both"
Returns
[X,Y,Z]
"""
if arm not in ['left', 'right', 'both']:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if arm == "left":
return list(self.left_arm.endpoint_pose()['position'])
elif arm == "right":
return list(self.right_arm.endpoint_pose()['position'])
elif arm == "both":
return list(self.left_arm.endpoint_pose()['position']) + list(
self.right_arm.endpoint_pose()['position'])
def get_ee_orientation(self, arm, mode=None):
"""
Returns a list of the orientations of the end effectors(s)
Args
arm (string): "right" or "left" or "both"
mode (string): specifies angle representation
Returns
orn (list): list of Euler angles or Quaternion
"""
if arm not in ['left', 'right', 'both']:
raise ValueError("Arg arm should be 'left' or 'right'.")
if arm == "left":
orn = list(self.left_arm.endpoint_pose()['orientation'])
elif arm == "right":
orn = list(self.right_arm.endpoint_pose()['orientation'])
elif arm == "both":
orn = list(self.left_arm.endpoint_pose()['orientation']) + list(
self.right_arm.endpoint_pose()['orientation'])
return list(p.getEulerFromQuaternion(orn))
def get_joint_angles(self, arm):
"""
Get joint angles for specified arm
Args
arm(strin): "right" or "left" or "both"
Returns
joint_angles (list): List of joint angles starting from the right_s0 ('right_upper_shoulder')
and going down the kinematic tree to the end effector.
"""
if arm == "left":
joint_angles = self.left_arm.joint_angles()
elif arm == "right":
joint_angles = self.right_arm.joint_angles()
elif arm == "both":
joint_angles = self.left_arm.joint_angles(
) + self.right_arm.joint_angles()
else:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if not self.sim:
joint_angles = joint_angles.values()
return joint_angles
def get_joint_velocities(self, arm):
"""
Get joint velocites for specified arm
"""
if arm == "left":
return self.left_arm.joint_velocities()
elif arm == "right":
return self.right_arm.joint_velocities()
elif arm == "both":
return self.left_arm.joint_velocities(
) + self.right_arm.joint_velocities()
else:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if not self.sim:
joint_velocities = joint_velocities.values()
return joint_velocities
def get_joint_efforts(self, arm):
"""
Get joint torques for specified arm
"""
if arm == "left":
return self.left_arm.joint_effort()
elif arm == "right":
return self.right_arm.joint_efforts()
elif arm == "both":
return self.left_arm.joint_efforts(
) + self.right_arm.joint_efforts()
else:
raise ValueError("Arg arm should be 'left' or 'right' or 'both'.")
if not self.sim:
joint_efforts = joint_efforts.values()
return
def apply_action(self, arm, control_type, action):
"""
Apply a joint action
Blocking on real robot
Args
action - list or tuple specifying end effector pose(6DOF * num_arms)
or joint angles (7DOF * num_arms)
"""
# verify action
verified, err = self._verify_action(arm, control_type, action)
if not verified:
raise err
# execute action
if control_type == 'position':
self._apply_position_control(arm, action)
elif control_type == 'ee':
self._apply_ee_control(arm, action)
elif control_type == 'velocity':
self._apply_velocity_control(arm, action)
elif control_type == 'torque':
self._apply_torque_control(arm, action)
else:
raise ValueError(
"Control type must be ['ee', 'position', 'velocity', 'torque']"
)
def _verify_action(self, arm, control_type, action):
"""
Verify type and dimension of action
Args
arm (str): "left" or "right" or "both"
action (list): list of floats len will vary depending on action type
Returns
bool: True if action is right dimension, false otherwise
"""
if arm not in ["left", "right", "both"]:
return False, ValueError("Arg arm must be string")
if control_type not in ['ee', 'position', 'velocity', 'torque']:
return False, ValueError(
"Arg control_type must be one of ['ee', 'position', 'velocity', 'torque']"
)
if not isinstance(action, (list, tuple, np.ndarray)):
return False, TypeError("Action must be a list or tuple.")
if len(action) != self.dof[arm][control_type]:
return False, ValueError("Action must have len {}".format(
self.dof * num_arms))
return True, ""
def _apply_torque_control(self, arm, action):
"""
As of right now, setting joint torques does not
does not command the robot as expected
"""
raise NotImplementedError(
'Cannot apply torque control. Try using joint position or velocity control'
)
def _apply_velocity_control(self, arm, action):
"""
Apply velocity control to a given arm
Args
arm (str): 'right' or 'left'
action (list, tuple, or numpy array) of len 7
"""
if self.sim:
pass
else:
if arm == 'left':
action_dict = self
self.right_arm.set_joint_velocities
if arm == 'right':
pass
if arm == 'both':
pass
return
def _apply_position_control(self, arm, action, blocking=True):
"""
Apply a joint action
Args:
arm (str): 'right', 'left', or 'both'
action - list or array of joint angles
blocking (bool): If true, wait for arm(s) to reach final position(s)
"""
action = list(action)
if blocking:
self.move_to_joint_positions(arm, action)
else:
self.set_joint_positions(arm, action)
return
def _apply_ee_control(self, arm, action, blocking=True):
"""
Apply action to move Baxter end effector(s)
Args
arm (str): 'left' or 'right' or 'both'
action (list, tuple, or numpy array)
blocking: Bool
"""
action = list(action)
if self.sim:
if arm == 'left' or arm == 'right':
self.move_to_ee_pose(arm, action)
elif arm == 'both':
if self.sim:
joint_indices = self.left_arm.indices + self.right_arm.indices
self.left_arm.move_to_joint_positions(
actions, joint_indices)
else:
self.move_to_ee_pose(arm, action, blocking)
return
def fk(self, arm, joints):
"""
Calculate forward kinematics
Args
arm (str): 'right' or 'left'
joints (list): list of joint angles
Returns
pose(list): [x,y,z,r,p,w]
"""
if arm == 'right':
pose = list(self.right_arm.kin.forward_position_kinematics(joints))
elif arm == 'left':
pose = list(self.left_arm.kin.forward_position_kinematics(joints))
else:
raise ValueError("Arg arm must be 'right' or 'left'")
return pose[:3] + list(self.quat_to_euler(pose[3:]))
def _ik(self, arm, pos, orn=None, seed=None):
"""
Calculate inverse kinematics
Args
arm (str): 'right' or 'left'
pos (list): [X, Y, Z]
orn (list): [r, p, w]
seed (int): for setting random seed
Returns
joint angles (list): A list of joint angles
Note: This will probably fail if orn is not included
"""
if orn is not None:
orn = list(self.euler_to_quat(orn))
if arm == 'right':
joint_angles = self.right_arm.kin.inverse_kinematics(pos, orn)
elif arm == 'left':
joint_angles = self.left_arm.kin.inverse_kinematics(pos, orn, seed)
else:
raise ValueError("Arg arm must be 'right' or 'left'")
return list(joint_angles.tolist())
def ik(self, arm, ee_pose):
"""
Calculate inverse kinematics for a given end effector pose
Args
ee_pose (list or tuple of floats): 6 dimensional array specifying
the position and orientation of the end effector
arm (string): "right" or "left"
Return
joints (list): A list of joint angles
"""
if self.sim:
joints = self._sim_ik(arm, ee_pose)
else:
joints = self._real_ik(arm, ee_pose)
if not joints:
print("IK failed. Try running again or changing the pose.")
return joints
def _sim_ik(self, arm, ee_pose):
"""
(Sim) Calculate inverse kinematics for a given end effector pose
Args:
ee_pose (tuple or list): [pos, orn] of desired end effector pose
pos - x,y,z
orn - r,p,w
arm (string): "right" or "left"
Returns:
joint_angles (list): List of joint angles
"""
if arm == 'right':
ee_index = self.right_arm.ee_index
elif arm == 'left':
ee_index = self.left_arm.ee_index
elif arm == 'both':
pass
else:
raise ValueError("Arg arm must be 'left', 'right', or 'both'.")
pos = ee_pose[:3]
orn = ee_pose[3:]
if (self.use_nullspace == 1):
if (self.use_orientation == 1):
joints = p.calculateInverseKinematics(self.baxter_id, ee_index,
pos, orn, self.ll,
self.ul, self.jr,
self.rp)
else:
joints = p.calculateInverseKinematics(self.baxter_id,
ee_index,
pos,
lowerLimits=self.ll,
upperLimits=self.ul,
jointRanges=self.jr,
restPoses=self.rp)
else:
if (self.use_orientation == 1):
joints = p.calculateInverseKinematics(self.baxter_id,
ee_index,
pos,
orn,
jointDamping=self.jd)
else:
joints = p.calculateInverseKinematics(self.baxter_id, ee_index,
pos)
return joints
def _real_ik(self, arm, ee_pose):
"""
(Real) Calculate inverse kinematics for a given end effector pose
Args:
ee_pose (tuple or list): [pos, orn] of desired end effector pose
pos - x,y,z
orn - r,p,w
Returns:
joint_angles (dict): Dictionary containing {'joint_name': joint_angle}
"""
pos = ee_pose[:3]
orn = ee_pose[3:]
orn = self.euler_to_quat(orn)
ns = "ExternalTools/" + arm + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ik_pose = PoseStamped()
ik_pose.pose.position.x = pos[0]
ik_pose.pose.position.y = pos[1]
ik_pose.pose.position.z = pos[2]
ik_pose.pose.orientation.x = orn[0]
ik_pose.pose.orientation.y = orn[1]
ik_pose.pose.orientation.z = orn[2]
ik_pose.pose.orientation.w = orn[3]
ik_pose.header = hdr
ikreq.pose_stamp.append(ik_pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
limb_joints = dict(
zip(resp.joints[0].name, resp.joints[0].position))
return limb_joints
except (rospy.ServiceException, rospy.ROSException), e:
# rospy.logerr("Service call failed: %s" % (e,))
return
class PoseGenerator(object):
def __init__(self, mode, arm_mode, step=5):
self.mode = mode
self.arm_mode = arm_mode
self._step = step
self._right_limb = Limb('right')
self._left_limb = Limb('left')
self._subscribe()
def _subscribe(self):
self._last_data_0 = (Vector3(), Vector3())
self._last_data_1 = (Vector3(), Vector3())
self._calib_data_0 = (Vector3(), Vector3())
self._calib_data_1 = (Vector3(), Vector3())
self._sub_ori_0 = rospy.Subscriber("/low_myo/imu_rpy",
Vector3,
self._orientation_callback_0)
self._sub_pos_0 = rospy.Subscriber("/low_myo/imu_pos",
Vector3,
self._position_callback_0)
self._sub_ori_1 = rospy.Subscriber("/top_myo/imu_rpy",
Vector3,
self._orientation_callback_1)
self._sub_pos_1 = rospy.Subscriber("/top_myo/imu_pos",
Vector3,
self._position_callback_1)
def _orientation_callback_0(self, data):
self._last_data_0[0].x = data.x
self._last_data_0[0].y = data.y
self._last_data_0[0].z = data.z
def _position_callback_0(self, data):
self._last_data_0[1].x = data.x
self._last_data_0[1].y = data.y
self._last_data_0[1].z = data.z
def _orientation_callback_1(self, data):
self._last_data_1[0].x = data.x
self._last_data_1[0].y = data.y
self._last_data_1[0].z = data.z
def _position_callback_1(self, data):
self._last_data_1[1].x = data.x
self._last_data_1[1].y = data.y
self._last_data_1[1].z = data.z
def calibrate(self):
"""
Calibrate position of the robot arm wrt the myo data
"""
raw_input("Press enter when user is at the right pose")
self._calib_data_0 = deepcopy(self._last_data_0)
self._calib_data_1 = deepcopy(self._last_data_1)
self._right_calib_pose = self._right_limb.joint_angles()
self._left_calib_pose = self._left_limb.joint_angles()
def _is_vector_valid(self, data):
"""
Check if data is likely to be valid.
"""
test = data.x + data.y + data.z
return test != 0
def _is_over_step(self, change):
return abs(change) > self._step
def generate_pose(self):
"""
Given new data and position of the arm, calculate new
joint positions.
"""
if self.mode == "one_arm":
return self.one_arm_generate_pose()
elif self.mode == "two_arms":
return self.two_arms_generate_pose()
else:
raise ValueError("Mode %s is invalid!" % self.mode)
def one_arm_generate_pose(self):
rospy.logdebug("Generating pose")
data_0 = deepcopy(self._last_data_0[0])
data_1 = deepcopy(self._last_data_1[0])
this_pose = deepcopy(self._right_calib_pose)
if self.arm_mode == "first":
if self._is_vector_valid(data_1):
change_1 = Vector3()
change_1.x = data_1.x - self._calib_data_1[0].x
change_1.y = data_1.y - self._calib_data_1[0].y
change_1.z = data_1.z - self._calib_data_1[0].z
if (change_1.x < -179):
change_1.x += 360
if (change_1.y < -179):
change_1.y += 360
if (change_1.z < -179):
change_1.z += 360
if (change_1.x > 179):
change_1.x -= 360
if (change_1.y > 179):
change_1.y -= 360
if (change_1.z > 179):
change_1.z -= 360
# print "MYO_1 (Upper arm)"
rospy.logdebug("Data_1.x %f" % data_1.x)
rospy.logdebug("CalibData_1.x %f" % self._calib_data_1[0].x)
rospy.logdebug("Data_1.y %f" % data_1.y)
rospy.logdebug("CalibData_1.y %f" % self._calib_data_1[0].y)
rospy.logdebug("Data_1.z %f" % data_1.z)
rospy.logdebug("CalibData_1.z %f" % self._calib_data_1[0].z)
# print "ROLL: ", change_1.z
if (self._is_over_step(change_1.z)):
this_pose["right_e0"] += radians(-1 * change_1.z)
# print "PITCH: ", change_1.y
if (self._is_over_step(change_1.y)):
# this_pose["right_e1"] += radians(-1 * change_1.y)
this_pose["right_s1"] += radians(1 * change_1.y)
# print "YAW: ", change_1.x
if (self._is_over_step(change_1.x)):
this_pose["right_s0"] += radians(-1 * change_1.x)
if self._is_vector_valid(data_0):
change_0 = Vector3()
change_0.x = data_0.x - self._calib_data_0[0].x - change_1.x
change_0.y = data_0.y - self._calib_data_0[0].y - change_1.y
change_0.z = data_0.z - self._calib_data_0[0].z - change_1.z
if (change_0.x < -179):
change_0.x += 360
if (change_0.y < -179):
change_0.y += 360
if (change_0.z < -179):
change_0.z += 360
if (change_0.x > 179):
change_0.x -= 360
if (change_0.y > 179):
change_0.y -= 360
if (change_0.z > 179):
change_0.z -= 360
# print "MYO_0 (Forearm)"
rospy.logdebug("Data_0.x %f" % data_0.x)
rospy.logdebug("CalibData_0.x %f" % self._calib_data_0[0].x)
rospy.logdebug("Data_0.y %f" % data_0.y)
rospy.logdebug("CalibData_0.y %f" % self._calib_data_0[0].y)
rospy.logdebug("Data_0.z %f" % data_0.z)
rospy.logdebug("CalibData_0.z %f" % self._calib_data_0[0].z)
# print "ROLL: ", change_0.z
if (self._is_over_step(change_0.z)):
this_pose["right_w0"] += radians(-1 * change_0.z)
# this_pose["right_w0"] += radians(1 * change_0.z)
# print "PITCH: ", change_0.y
if (self._is_over_step(change_0.y)):
# this_pose["right_w1"] += radians(-1 * change_0.y)
# this_pose["right_w1"] += radians(-1 * change_0.y)
this_pose["right_w1"] += radians(1 * change_0.y)
# print "YAW: ", change_0.x
if (self._is_over_step(change_0.x)):
# this_pose["right_w2"] += radians(-1 * change_0.x)
# this_pose["right_w2"] += radians(1 * change_0.x)
this_pose["right_e1"] += radians(-1 * change_0.x)
elif self.arm_mode == "second":
print "SECOND!"
# Alex you need to change below
if self._is_vector_valid(data_1):
change_1 = Vector3()
change_1.x = data_1.x - self._calib_data_1[0].x
change_1.y = data_1.y - self._calib_data_1[0].y
change_1.z = data_1.z - self._calib_data_1[0].z
print "MYO_1 (Upper arm)"
print "ROLL: ", change_1.x
if (self._is_over_step(change_1.x)):
this_pose["right_e0"] += radians(change_1.x)
print "PITCH: ", data_1.y
if (self._is_over_step(change_1.y)):
this_pose["right_s1"] += radians(-1 * change_1.y)
print "YAW: ", change_1.z
if (self._is_over_step(change_1.z)):
this_pose["right_s0"] += radians(change_1.z)
if self._is_vector_valid(data_0):
change_0 = Vector3()
change_0.x = data_0.x - self._calib_data_0[0].x
change_0.y = data_0.y - self._calib_data_0[0].y
change_0.z = data_0.z - self._calib_data_0[0].z
print "MYO_0 (Forearm)"
print "PITCH: ", data_0.y
print "ROLL: ", data_0.x
if (self._is_over_step(change_0.x)):
this_pose["right_w2"] += radians(-1 * change_0.x)
if (self._is_over_step(change_0.y)):
change_0.y += change_1.y
this_pose["right_e1"] += radians(-1 * change_0.y)
print "YAW: ", data_0.z
if (self._is_over_step(change_0.z)):
this_pose["right_w1"] += radians(change_0.z)
return this_pose
def two_arms_generate_pose(self):
rospy.logdebug("Generating pose")
data_0 = deepcopy(self._last_data_0[0])
data_1 = deepcopy(self._last_data_1[0])
right_pose = deepcopy(self._right_calib_pose)
left_pose = deepcopy(self._left_calib_pose)
if self.arm_mode == "first":
if self._is_vector_valid(data_0):
change_0 = Vector3()
change_0.x = data_0.x - self._calib_data_0[0].x
change_0.y = data_0.y - self._calib_data_0[0].y
change_0.z = data_0.z - self._calib_data_0[0].z
print "MYO_0 (Right forearm)"
print "PITCH: ", change_0.y
if (self._is_over_step(change_0.y)):
right_pose["right_w1"] += radians(-1 * change_0.y)
print "YAW: ", change_0.z
if (self._is_over_step(change_0.z)):
right_pose["right_w0"] += radians(-1 * change_0.z)
print "ROLL: ", change_0.x
if (self._is_over_step(change_0.x)):
right_pose["right_w2"] += radians(-1 * change_0.x)
if self._is_vector_valid(data_1):
change_1 = Vector3()
change_1.x = data_1.x - self._calib_data_1[0].x
change_1.y = data_1.y - self._calib_data_1[0].y
change_1.z = data_1.z - self._calib_data_1[0].z
print "MYO_1 (Left forearm)"
print "PITCH: ", change_1.y
if (self._is_over_step(change_1.y)):
left_pose["left_w1"] += radians(-1 * change_1.y)
print "YAW: ", change_1.z
if (self._is_over_step(change_1.z)):
left_pose["left_w0"] += radians(-1 * change_1.z)
print "ROLL: ", change_1.x
if (self._is_over_step(change_1.x)):
left_pose["left_w2"] += radians(-1 * change_1.x)
elif self.arm_mode == "second":
if self._is_vector_valid(data_0):
change_0 = Vector3()
change_0.x = data_0.x - self._calib_data_0[0].x
change_0.y = data_0.y - self._calib_data_0[0].y
change_0.z = data_0.z - self._calib_data_0[0].z
print "MYO_0 (Right forearm)"
print "PITCH: ", change_0.y
if (self._is_over_step(change_0.y)):
right_pose["right_e1"] += radians(change_0.y)
print "YAW: ", change_0.z
if (self._is_over_step(change_0.z)):
right_pose["right_w1"] += radians(change_0.z)
print "ROLL: ", change_0.x
if (self._is_over_step(change_0.x)):
right_pose["right_w2"] += radians(-1 * change_0.x)
if self._is_vector_valid(data_1):
change_1 = Vector3()
change_1.x = data_1.x - self._calib_data_1[0].x
change_1.y = data_1.y - self._calib_data_1[0].y
change_1.z = data_1.z - self._calib_data_1[0].z
print "MYO_1 (Left forearm)"
print "PITCH: ", change_1.y
if (self._is_over_step(change_1.y)):
left_pose["left_e1"] += radians(-1 * change_1.y)
print "YAW: ", change_1.z
if (self._is_over_step(change_1.z)):
left_pose["left_w1"] += radians(-1 * change_1.z)
print "ROLL: ", change_1.x
if (self._is_over_step(change_1.x)):
left_pose["left_w2"] += radians(change_1.x)
return (right_pose, left_pose)
def main():
rospy.init_node('baxter_kinematics')
kin = baxter_kinematics('left')
pos = [0.582583, -0.180819, 0.216003]
rot = [0.03085, 0.9945, 0.0561, 0.0829]
fl = fcntl.fcntl(sys.stdin.fileno(), fcntl.F_GETFL)
fcntl.fcntl(sys.stdin.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK)
# Read initial positions:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm=Limb('right')
left_arm=Limb('left')
joints=left_arm.joint_names()
positions=convert_dict_to_list('left',left_arm.joint_angles())
velocities=convert_dict_to_list('left',left_arm.joint_velocities())
force=convert_dict_to_list('left',left_arm.joint_efforts())
positions=np.reshape(positions,[7,1]) # Converts to matrix
velocities=np.reshape(velocities,[7,1]) # Converts to matrix
force=np.reshape(force,[7,1]) # Converts to matrix
# Initial states
q_previous=positions # Starting joint angles
q_dot_previous=velocities # Starting joint velocities
x_previous=kin.forward_position_kinematics() # Starting end effector position
x_dot_previous=np.zeros((6,1))
# Set model parameters:
C=50
B=1
f_des=np.reshape(np.array([0,15,0,0,0,0]),[6,1])
J=kin.jacobian()
J_T=kin.jacobian_transpose()
J_PI=kin.jacobian_pseudo_inverse()
J_T_PI=np.linalg.pinv(J_T)
x=kin.forward_position_kinematics()
x=x[:-1]
x_ref=np.reshape(x,[6,1]) # Reference position
x_ref_dot=J*velocities # Reference velocities
t_stop=15
t_end=time.time()+t_stop # Loop timer (in seconds)
# Initial plot parameters
time_cum=0
time_vec=[time_cum]
f=J_T_PI*force
force_vec=[convert_mat_to_list(f[1])[0]]
while time.time()<t_end:
from sensor_msgs.msg import JointState
from baxter_interface import Limb
right_arm=Limb('right')
left_arm=Limb('left')
J=kin.jacobian()
J_T=kin.jacobian_transpose()
J_PI=kin.jacobian_pseudo_inverse()
J_T_PI=np.linalg.pinv(J_T)
joints=left_arm.joint_names()
positions=convert_dict_to_list('left',left_arm.joint_angles())
velocities=convert_dict_to_list('left',left_arm.joint_velocities())
force=convert_dict_to_list('left',left_arm.joint_efforts())
positions=np.reshape(positions,[7,1]) # Converts to matrix
velocities=np.reshape(velocities,[7,1]) # Converts to matrix
force=np.reshape(force,[7,1]) # Converts to matrix
x=kin.forward_position_kinematics()
x=x[:-1]
x_current=np.reshape(x,[6,1])
x_dot_current=J*velocities
# Only interested in y-axis:
x_dot_current[0]=0
#x_dot_current[1]=0
x_dot_current[2]=0
x_dot_current[3]=0
x_dot_current[4]=0
x_dot_current[5]=0
# Model goes here
f=J_T_PI*force # spacial force
# Only interested in y-axis:
f[0]=[0]
#f[1]=[0]
f[2]=[0]
f[3]=[0]
f[4]=[0]
f[5]=[0]
x_dot_des=-B*(f_des+f)/C # Impedance control
# Control robot
q_dot_ctrl=J_PI*x_dot_des # Use this for damper system
q_dot_ctrl=np.multiply(q_dot_ctrl,np.reshape(np.array([1,0,0,0,0,0,0]),[7,1])) # y-axis translation corresponds to first shoulder joint rotation
q_dot_ctrl_list=convert_mat_to_list(q_dot_ctrl)
q_dot_ctrl_dict=convert_list_to_dict(joints,q_dot_ctrl_list)
left_arm.set_joint_velocities(q_dot_ctrl_dict) # Velocity control function
# Update values before next loop
x_previous=x_current # Updates previous position for next loop iteration
x_dot_previous=x_dot_current # Updates previous velocity for next loop iteration
# Update plot info
time_cum+=.05
time_vec.append(time_cum)
force_vec.append(convert_mat_to_list(f[1])[0])
print(time_vec)
print(force_vec)
plt.plot(time_vec,force_vec)
plt.title('Force applied over time')
plt.xlabel('Time (sec)')
plt.ylabel('Force (N)')
plt.show()
time.sleep(1)