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()
ikreq.pose_stamp.append(poses['right'])
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return 1
if (resp.isValid[0]):
print("SUCCESS - Valid Joint Solution Found:")
# Format solution into Limb API-compatible dictionary
#limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
#print limb_joints
arm = Limb('right')
i = 0
while i < 300:
arm.set_joint_positions(limb_joints)
i += 1
rospy.sleep(0.01)
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return 0
if __name__ == '__main__':
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq.pose_stamp.append(pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return 0
if (resp.isValid[0]):
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
if side == "right": arm = Limb("right")
else: arm = Limb("left")
arm.set_joint_positions(limb_joints)
return 1
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return 0
def ik_joint(pose, side):
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
if side == "left":
ns = "ExternalTools/left/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
def ik_move(side,
pub,
jacob,
control_mode='position',
target_dx=None,
target_dy=None,
target_dz=None,
x=None,
y=None,
z=None,
timeout=5,
speed=1.0):
if side == "right": arm = Limb("right")
else: arm = Limb("left")
initial_pose = get_current_pose(arm)
target_x = initial_pose.pose.position.x
target_y = initial_pose.pose.position.y
target_z = initial_pose.pose.position.z
if target_dx != None: target_x += target_dx
if target_dy != None: target_y += target_dy
if target_dz != None: target_z += target_dz
if x != None: target_x = x
if y != None: target_y = y
if z != None: target_z = z
dx = 100
dy = 100
dz = 100
solution_found = 1
start = rospy.get_time()
while (abs(dz) > 0.01) and solution_found == 1 and (
rospy.get_time() - start) < timeout and not rospy.is_shutdown():
# while (abs(dx) > 0.01 or abs(dy) > 0.01 or abs(dz) > 0.01) and solution_found == 1 and (rospy.get_time() - start) < timeout:
#while (abs(dx) > 0.01 or abs(dy) > 0.01 or abs(dz) > 0.01) and i < 5000:
current_pose = get_current_pose(arm, initial_pose)
current_x = current_pose.pose.position.x
current_y = current_pose.pose.position.y
current_z = current_pose.pose.position.z
dx = target_x - current_x
dy = target_y - current_y
dz = target_z - current_z
#vx = np.sign(dx)*min(0.02,abs(dx))
#vy = np.sign(dy)*min(0.02,abs(dy))
#vz = np.sign(dz)*min(0.02,abs(dz))
vx = dx * speed
vy = dy * speed
vz = dz * speed
#print dx, dy, dz
p = Point()
p.x = vx * 1e6
p.y = vy * 1e6
p.z = vz * 1e6
pub.publish(p)
if control_mode is 'velocity': velocity_control(arm, jacob, vx, vy, vz)
else: position_control(side, current_pose, vx, vy, vz)
return solution_found
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper_left = Gripper("left")
self.gripper_left.calibrate()
self.gripper_left.set_moving_force(0.01)
rospy.sleep(0.5)
self.gripper_left.set_holding_force(0.01)
rospy.sleep(0.5)
self.gripper_right = Gripper("right")
self.gripper_right.calibrate()
rospy.sleep(1)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin_right = baxter_kinematics('right')
self.kin_left = baxter_kinematics('left')
self.J = self.kin_right.jacobian()
self.J_inv = self.kin_right.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.left_arm = Limb("left")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
self.pour_x = 0
self.pour_y = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
self.joint_states = {
'observe':{
'right_e0': -0.631,
'right_e1': 0.870,
'right_s0': 0.742,
'right_s1': -0.6087,
'right_w0': 0.508,
'right_w1': 1.386,
'right_w2': -0.5538,
},
'observe_ladle':{
'right_e0': -0.829,
'right_e1': 0.831,
'right_s0': 0.678,
'right_s1': -0.53,
'right_w0': 0.716,
'right_w1': 1.466,
'right_w2': -0.8099,
},
'observe_left':{
'left_w0': 2.761932405432129,
'left_w1': -1.5700293346069336,
'left_w2': -0.8893253607604981,
'left_e0': -0.9805972175354004,
'left_e1': 1.8300390778564455,
'left_s0': 1.4783739826354982,
'left_s1': -0.9503010970092775,
},
'stir':{
'right_e0': -0.179,
'right_e1': 1.403,
'right_s0': 0.381,
'right_s1': -0.655,
'right_w0': 1.3,
'right_w1': 2.04,
'right_w2': 0.612,
},
'observe_vertical':{
'right_e0': 0.699,
'right_e1': 1.451,
'right_s0': -1.689,
'right_s1': 0.516,
'right_w0': 0.204,
'right_w1': 0.935,
'right_w2': -2.706,
},
'observe_midpoint':{
'right_e0': -0.606,
'right_e1': 0.968,
'right_s0': 0.0,
'right_s1': -0.645,
'right_w0': 0.465,
'right_w1': 1.343,
'right_w2': -0.437,
},
'dressing':{
'right_e0': 0.967,
'right_e1': 1.386,
'right_s0': -0.348,
'right_s1': -0.155,
'right_w0': 0.264,
'right_w1': 1.521,
'right_w2': -2.199,
},
}
def main():
joint_states = {
# 'observe':{
# 'right_e0': -0.365,
# 'right_e1': 1.061,
# 'right_s0': 0.920,
# 'right_s1': -0.539,
# 'right_w0': 0.350,
# 'right_w1': 1.105,
# 'right_w2': -0.221,
# },
'observe':{
'right_e0': -0.631,
'right_e1': 0.870,
'right_s0': 0.742,
'right_s1': -0.6087,
'right_w0': 0.508,
'right_w1': 1.386,
'right_w2': -0.5538,
},
'observe_ladle':{
'right_e0': -0.829,
'right_e1': 0.831,
'right_s0': 0.678,
'right_s1': -0.53,
'right_w0': 0.716,
'right_w1': 1.466,
'right_w2': -0.8099,
},
'observe_left':{
'left_w0': 2.761932405432129,
'left_w1': -1.5700293346069336,
'left_w2': -0.8893253607604981,
'left_e0': -0.9805972175354004,
'left_e1': 1.8300390778564455,
'left_s0': 1.4783739826354982,
'left_s1': -0.9503010970092775,
},
'stir':{
'right_e0': -0.179,
'right_e1': 1.403,
'right_s0': 0.381,
'right_s1': -0.655,
'right_w0': 1.3,
'right_w1': 2.04,
'right_w2': 0.612,
},
'observe_vertical':{
'right_e0': 0.699,
'right_e1': 1.451,
'right_s0': -1.689,
'right_s1': 0.516,
'right_w0': 0.204,
'right_w1': 0.935,
'right_w2': -2.706,
},
'observe_midpoint':{
'right_e0': -0.606,
'right_e1': 0.968,
'right_s0': 0.0,
'right_s1': -0.645,
'right_w0': 0.465,
'right_w1': 1.343,
'right_w2': -0.437,
},
'dressing':{
'right_e0': 0.967,
'right_e1': 1.386,
'right_s0': -0.348,
'right_s1': -0.155,
'right_w0': 0.264,
'right_w1': 1.521,
'right_w2': -2.199,
},
'opening_right':{
'right_e0': 0.40075,
'right_e1': 0.82145,
'right_s0': 0.86133,
'right_s1': -0.22281,
'right_w0': 1.31654,
'right_w1': 1.21338,
'right_w2': 0.98942,
},
'opening_left':{
'left_e0': -0.2197,
'left_e1': 0.8583,
'left_s0': -0.6493,
'left_s1': -0.9410,
'left_w0': 0.11888,
'left_w1': 1.611833,
'left_w2': -0.19788,
},
}
roscpp_initialize(sys.argv)
rospy.init_node('open_cap_demo', anonymous=True)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
left_arm = Limb("left")
right_arm = Limb("right")
set_joints(target_angles_right = joint_states['observe_midpoint'],target_angles_left = joint_states['observe_left'],timeout= 100000)
tracker=track()
rospy.on_shutdown(tracker.clean_shutdown)
#pour tomato
set_joints(target_angles_right = joint_states['observe'])
initial_pose = get_current_pose(hdr,right_arm)
speak("I am looking for the strawberry")
tracker.track(initial_pose, hdr, right_arm, id = 1)
tracker.gripper_right.close()
rospy.sleep(2)
ik_move(hdr,right_arm, target_dz = 0.25)
tracker.pour_tomato()
ik_move(hdr,right_arm, target_dz = -0.22, speedz = 0.2, timeout = 2)
tracker.gripper_right.open()
rospy.sleep(0.5)
ik_move(hdr,right_arm, target_dy = -0.05,target_dz = 0.20)
# Dressing
set_joints(target_angles_right = joint_states['observe_midpoint'])
speak("I am looking for the dressing")
set_joints(target_angles_right = joint_states['observe_vertical'],timeout= 400000)
initial_pose = get_current_pose(hdr,right_arm)
tracker.track(initial_pose, hdr, right_arm, id = 4)
tracker.gripper_right.close()
rospy.sleep(1)
ik_move(hdr,right_arm, side = "right", target_dz = 0.25, timeout = 3)
start_pose = get_current_pose(hdr, right_arm)
tracker.pour_x, tracker.pour_y = tracker.moveToBowl()
# open_cap
rospy.sleep(0.1)
left_pose = PoseStamped()
left_pose.header = hdr
left_pose.pose.position.x = 0.70
left_pose.pose.position.y = 0.264
left_pose.pose.position.z = 0.389
left_pose.pose.orientation.x = 0
left_pose.pose.orientation.y = 1
left_pose.pose.orientation.z = 0
left_pose.pose.orientation.w = 0
right_pose = PoseStamped()
right_pose.header = hdr
right_pose.pose.position.x = 0.705
right_pose.pose.position.y = 0.21
right_pose.pose.position.z = 0.19
right_pose.pose.orientation.x = -0.5
right_pose.pose.orientation.y = 0.5
right_pose.pose.orientation.z = 0.5
right_pose.pose.orientation.w = 0.5
move_to_pose(left_pose = left_pose, right_pose = right_pose, timeout = 8)
rospy.sleep(2)
dist = 0.06
ik_move(hdr,left_arm, target_dz = -dist, side = "left", speedz = 0.1)
rospy.sleep(2)
tracker.open()
tracker.rotate_left(-math.pi/2)
rospy.sleep(0.5)
tracker.gripper_left.close()
rospy.sleep(1)
ik_move(hdr,left_arm, target_dz = +dist, side = "left", speedz = 1)
#rospy.sleep(2)
tracker.pour_dressing(joint_states['dressing'])
move_to_pose( right_pose = right_pose, timeout = 8)
ik_move(hdr,left_arm, target_dz = -(dist+0.02), side = "left", speedz = 0.1, timeout = 5)
# tracker.gripper_left.open()
# rospy.sleep(1)
tracker.open(close = True, rotation = 2)
ik_move(hdr,left_arm, target_dz = +dist + 0.02, side = "left", speedz = 0.3, timeout = 3)
rospy.sleep(2)
set_joints(target_angles_right = joint_states['dressing'],timeout= 400000)
move_to_pose(right_pose = start_pose, timeout = 5)
ik_move(hdr,right_arm, side = "right", target_dz = -0.25, timeout = 5)
tracker.gripper_right.open()
rospy.sleep(1)
ik_move(hdr,right_arm, side = "right", x = 0.35, speedx = 0.1, timeout = 3)
set_joints(target_angles_right = joint_states['observe_midpoint'],target_angles_left = joint_states['observe_left'])
# stir salad
set_joints(target_angles_right = joint_states['observe_ladle'])
speak("I am looking for the ladle")
initial_pose = get_current_pose(hdr,right_arm)
tracker.track(initial_pose, hdr, right_arm, id = 2)
tracker.gripper_right.close()
rospy.sleep(2)
ik_move(hdr,right_arm, target_dz = 0.23)
tracker.stir_salad(joint_states['stir'])
ik_move(hdr,right_arm, target_dz = -0.23, speedz = 0.2, timeout = 5)
tracker.gripper_right.open()
rospy.sleep(0.5)
ik_move(hdr,right_arm, target_dz = 0.25)
speak("I am done with the task")
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
ikreq.pose_stamp.append(pose)
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return False
if (resp.isValid[0]):
#print("SUCCESS - Valid Joint Solution Found:")
# Format solution into Limb API-compatible dictionary
limb_joints = dict(zip(resp.joints[0].name, resp.joints[0].position))
arm = Limb("right")
arm.set_joint_positions(limb_joints)
return True
#rospy.sleep(0.05)
else:
print("INVALID POSE - No Valid Joint Solution Found.")
return False
def ik_pykdl(arm, kin, pose, arm_position='right'):
position = pose.pose.position
orientation = pose.pose.orientation
pos = [position.x, position.y, position.z]
rot = [orientation.x, orientation.y, orientation.z, orientation.w]
joint_angles = kin.inverse_kinematics(pos, rot)
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
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)
import sys
import time
import rospy
from std_msgs.msg import UInt32
from msg_types import *
import baxter_interface
from baxter_interface import Gripper, Head, Limb
rospy.init_node('run_condition')
voice_pub = rospy.Publisher('/voice', UInt32, queue_size=10)
time.sleep(1)
rightg = Gripper('right')
rightg.set_holding_force(50)
leftg = Gripper('left')
right = Limb('right')
left = Limb('left')
head = Head()
neutral_right = {
'right_s0': 0.476301034638421,
'right_s1': -0.5606699779721187,
'right_w0': 0.07094661143970038,
'right_w1': 0.7037136864424336,
'right_w2': -0.28033498898605935,
'right_e0': -0.16566992509162468,
'right_e1': 1.3077186216723151
}
neutral_left = {
'left_w0': -0.15339807878854136,
'left_w1': 0.34552917247118947,
'left_w2': 3.0158062289827234,
rospy.loginfo("Returning arms back to untucked position.")
tucker = tuck_arms.Tuck(False)
rospy.on_shutdown(tucker.clean_shutdown)
try:
tucker.supervised_tuck()
except Exception, e:
rospy.logerr(e.strerror)
rospy.logerr("Failed to return arms to untucked position.")
return False
rospy.loginfo("Arms in untucked position.")
rospy.loginfo("Moving arms to neutral position.")
left_arm_neutral = rospy.get_param(LEFT_ARM_NEUTRAL)
right_arm_neutral = rospy.get_param(RIGHT_ARM_NEUTRAL)
arm_l = Limb("left")
arm_r = Limb("right")
arm_l.move_to_joint_positions(left_arm_neutral)
arm_r.move_to_joint_positions(right_arm_neutral)
return True
def callback(self, request=True):
"""
Checks the status of the grasping components.
:param request: request from brain to check the handling
:return: True if all checks are good and 'False' otherwise
"""
def main():
joint_states = {
'right_initial':{
'right_e0': 2.498,
'right_e1': 2.158,
'right_s0': 0.826,
'right_s1': 0.366,
'right_w0': 2.809,
'right_w1': 1.867,
'right_w2': 0.411,
},
'left_initial':{
'left_e0': -1.738,
'left_e1': 1.828,
'left_s0': 0.247,
'left_s1': 0.257,
'left_w0': 0.0721,
'left_w1': -0.818,
'left_w2': 1.74,
},
}
roscpp_initialize(sys.argv)
rospy.init_node('demo', anonymous=True)
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
arm_right = Limb("right")
kin_right = baxter_kinematics('right')
arm_left = Limb("left")
kin_left = baxter_kinematics('left')
tracker=track()
#set_joints(target_angles_right = joint_states['observe_midpoint'],target_angles_left = joint_states['observe_left'],timeout= 100000)
rospy.on_shutdown(tracker.clean_shutdown)
#pour tomato
set_joints(target_angles_left = joint_states['left_initial'])
#initial_pose = get_current_pose(hdr,arm)
rospy.sleep(10)
# left arm movement
ik_move(hdr,arm_left, kin_left, target_dy = -0.054, arm_position = 'left', speedy = 0.1)
#rospy.sleep(10)
# ik_move(hdr,arm_left, kin_left, target_dz = 0.03, arm_position = 'left', speedz = 0.1)
# ik_move(hdr,arm_left, kin_left, target_dy = -0.06, arm_position = 'left', speedy = 0.1)
ik_move(hdr,arm_left, kin_left, target_dx = 0.160, arm_position = 'left', speedx = 0.1)
#ik_move(hdr,arm_left, kin_left, target_dz = -0.170, arm_position = 'left', speedz = 0.1)
set_joints(target_angles_right = joint_states['right_initial'])
# right arm movement
ik_move(hdr,arm_right, kin_right, target_dx = 0.35, arm_position = 'right', speedx = 0.05)
ik_move(hdr,arm_right, kin_right, target_dy = 0.1, arm_position = 'right', speedy = 0.1)
ik_move(hdr,arm_right, kin_right, target_dx = -0.35, arm_position = 'right', speedx = 0.1)
ik_move(hdr,arm_right, kin_right, target_dz = 0.2, arm_position = 'right', speedz = 0.1)
def __init__(self):
self.centerx = 365
self.centery = 120
self.coefx = 0.1/(526-369)
self.coefy = 0.1/(237-90)
self.count = 0
self.hdr = Header(stamp=rospy.Time.now(), frame_id='base')
self.gripper = Gripper("right")
#self.gripper.calibrate()
rospy.sleep(2)
# self.gripper.set_moving_force(0.1)
# rospy.sleep(0.5)
# self.gripper.set_holding_force(0.1)
# rospy.sleep(0.5)
self.busy = False
self.gripper_distance = 100
self.subscribe_gripper()
self.robotx = -1
self.roboty = -1
self.framenumber = 0
self.history = np.arange(0,20)*-1
self.newPosition = True
self.bowlcamera = None
self.kin = baxter_kinematics('right')
self.J = self.kin.jacobian()
self.J_inv = self.kin.jacobian_pseudo_inverse()
self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32)
self.control_arm = Limb("right")
self.control_joint_names = self.control_arm.joint_names()
self.dx = 0
self.dy = 0
self.distance = 1000
self.finish = False
self.found = 0
ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
self.joint_states = {
# 'observe':{
# 'right_e0': -0.365,
# 'right_e1': 1.061,
# 'right_s0': 0.920,
# 'right_s1': -0.539,
# 'right_w0': 0.350,
# 'right_w1': 1.105,
# 'right_w2': -0.221,
# },
'observe':{
'right_e0': -0.631,
'right_e1': 0.870,
'right_s0': 0.742,
'right_s1': -0.6087,
'right_w0': 0.508,
'right_w1': 1.386,
'right_w2': -0.5538,
},
'observe_vertical':{
'right_e0': 0.699,
'right_e1': 1.451,
'right_s0': -1.689,
'right_s1': 0.516,
'right_w0': 0.204,
'right_w1': 0.935,
'right_w2': -2.706,
},
}
