def grasp(key,
x,
y,
z,
x_threshhold=0.02,
y_threshhold=0.02,
hoverDist=0.020,
zoffset=.92,
orien_const=[],
or_x=0.0,
or_y=-1.0,
or_z=0.0,
or_w=0.0):
#while not rospy.is_shutdown():
try:
left_arm_planner = PathPlanner("left_arm")
right_arm_planner = PathPlanner("right_arm")
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x + x_threshhold
goal.pose.position.y = y + y_threshhold
goal.pose.position.z = z - zoffset + hoverDist
#Orientation as a quaternion
goal.pose.orientation.x = or_x
goal.pose.orientation.y = or_y
goal.pose.orientation.z = or_z
goal.pose.orientation.w = or_w
if key == 'left_arm':
plan = left_arm_planner.plan_to_pose(goal, orien_const)
openLeftGripper()
left_arm_planner.execute_plan(plan)
closeLeftGripper()
else:
plan = right_arm_planner.plan_to_pose(goal, orien_const)
openRightGripper()
right_arm_planner.execute_plan(plan)
closeRightGripper()
except Exception as e:
print e
traceback.print_exc()
def play_chords(note_pos):
planner = PathPlanner("left_arm")
new_note_pos = []
num_waypoints_per_note = 4
# add waypoints above the note to ensure the note is struck properly
for note in note_pos:
waypoint1 = Vector3()
waypoint1.x = note.x
waypoint1.y = note.y
waypoint1.z = note.z + waypoint_offset
waypoint2 = Vector3()
waypoint2.x = note.x
waypoint2.y = note.y
waypoint2.z = note.z + waypoint_offset / 2.0
new_note_pos += [waypoint1, waypoint2, note, waypoint1]
for i, pos in enumerate(new_note_pos):
print(i, pos)
# Loop until that position is reached
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = pos.x
goal_1.pose.position.y = pos.y
goal_1.pose.position.z = pos.z
# #Orientation as a quaternion, facing straight down
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
plan = planner.plan_to_pose(goal_1, list())
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
## TF CODE
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
## TF CODE
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
X = 0.40
Y = 1.20
Z = 0.10
Xp = 0.5
Yp = 0.00
Zp = -0.15
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
#planner.add_box_obstacle([X,Y,Z], "wall", pose)
#Wall 2
X = 0.40
Y = 0.10
Z = 0.30
Xp = 0.5
Yp = -0.15
Zp = 0.05
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
#planner.add_box_obstacle([X,Y,Z], "wall2", pose)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
#orien_const.orientation.y = -1.0;
#orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.05
orien_const.absolute_z_axis_tolerance = 0.05
orien_const.weight = 1.0
waypoints = list()
# Section to add waypoints
# step_size = size between x position changes
# final_length = final x position
#GOAL Points
# goal_1.pose.position.x = 0.585
# goal_1.pose.position.y = 0.156
# goal_1.pose.position.z = -0.138
# goal_1.pose.orientation.x = 0
# goal_1.pose.orientation.y = 0.707
# goal_1.pose.orientation.z = 0
# goal_1.pose.orientation.w = 0.707
#GOAL Points
while not rospy.is_shutdown():
## ZERO THE ROBOT ##
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = 0.585
goal_1.pose.position.y = 0.156
goal_1.pose.position.z = -0.138
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
waypoints = list()
## ZERO THE ROBOT ##
print("ROBOT ZEROED")
#control._velocity_scalar = 0.5
step_size = 0.0005 #0.0025
FINAL_X = 0.785 #
offset = 0.14
tf_px = 0.585
tf_py = 0.156
tf_pz = -0.138
tf_rx = 0
tf_ry = 0.707
tf_rz = 0
tf_rw = 0.707
goal_x = 0.585
goal_y = 0.156
goal_z = -0.138
try:
trans = tfBuffer.lookup_transform('base', 'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
goal_x = tf_px
goal_y = tf_py
goal_z = tf_pz
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
offset = 0.14
FINAL_X = goal_x + offset + 0.2
x = tf_px + offset
print("TF'd zero")
update = 0
execute = 0
i = 0
STOP = 0
while ((x < FINAL_X or update == 0) and (i < 5)):
if (update == 0):
try:
trans = tfBuffer.lookup_transform('base',
'right_gripper_base',
rospy.Time())
tf_px = trans.transform.translation.x
tf_py = trans.transform.translation.y
tf_pz = trans.transform.translation.z
tf_rx = trans.transform.rotation.x
tf_ry = trans.transform.rotation.y
tf_rz = trans.transform.rotation.z
tf_rw = trans.transform.rotation.w
x = tf_px + offset
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
update = 1
execute = 0
y = tf_py
z = tf_pz
if (y < goal_y - 0.05):
y = y + step_size
if (y > goal_y + 0.05):
y = y - step_size
if (z < goal_z - 0.05):
z = z + step_size
if (z > goal_z + 0.05):
z = z - step_size
#print("TF'd while")
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
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.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
x = x + step_size
if (x >= FINAL_X):
execute = 1
#print("added waypoint")
if (execute == 1):
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
#STOP = control._flag
waypoints = list()
update = 0
print("EXEcute")
print(i)
i = i + 1
print("END OF LOOP")
print("ZERO AGAIN")
control._zero = 1
waypoints = list()
## ZERO THE ROBOT ##
control._flag = 0
control._trigger = 0
control._velocity_scalar = 1
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
goal_1.pose.position.x = 0.585
goal_1.pose.position.y = 0.156
goal_1.pose.position.z = -0.138
goal_1.pose.orientation.x = 0 # -0.026
goal_1.pose.orientation.y = 0.707 # 0.723
goal_1.pose.orientation.z = 0 # -0.052
goal_1.pose.orientation.w = 0.707 # 0.689
waypoints.append(goal_1.pose)
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(waypoints, list())
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
waypoints = list()
control._zero = 0
## ZERO THE ROBOT ##
print("ROBOT ZEROED...AGAIN")
#plt.plot(control._sensor_data)
s = list(map(float, control._sensor_data))
scales = list(map(float, control._scalar_data))
goal_line = list(map(float, control._goal_list))
#s = list(map(float, control._debug))
#control._sensor_data = sorted(control._sensor_data)
##print(min(s))
##print(max(s))
plt.subplot(211)
plt.plot(s)
plt.plot(goal_line)
plt.axis([0, len(control._sensor_data), min(s), max(s)])
plt.ylabel('Sensor Data')
plt.subplot(212)
plt.plot(scales)
plt.show()
control._sensor_data = list()
control._scalar_data = list()
control._goal_list = list()
waypoints = list()
control._velocity_scalar = 1
# #Orientation as a quaternion
# table_pose.pose.orientation.w = 1.0
# planner.add_box_obstacle(table_size, "table", table_pose)
# cubes = [(0.48, -0.46, "red"), (0.486, -0.46, "blue"), (0.486, -0.46, "green")]
while not rospy.is_shutdown():
#Move right arm to default pose.
cnt = 0
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(default_pose, [])
# print(plan)
raw_input("Press <Enter> to move the right arm to default state: ")
result = planner.execute_plan(plan)
# print(result)
if not result:
raise Exception("Execution failed")
except Exception as e:
print(e)
else:
break
raw_input("Press enter if camera correctly positioned")
#Get positions of cubes.
message = rospy.wait_for_message("/colors_and_position", ColorAndPositionPairs)
cubes = message.pairs
def main():
"""
Main Script
"""
#############
offset_p = -0.07
offset_g = 0.04
#############
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("left_arm")
##
## Init Gripper
##
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
left = baxter_interface.Gripper('left', CHECK_VERSION)
tf_listener = tf.TransformListener(rospy.Duration(1))
raw_input("Press <Enter> to start!")
try_another_grasp = 0
good_grasps = rospy.get_param('good_grasps')
for g in range(good_grasps):
i_pregrasp = 0
## get grasp pose
tf_listener.waitForTransform('base', "sample_grasp_" + str(g),
rospy.Time(0), rospy.Duration(2.0))
t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(g),
rospy.Time())
matrix = quaternion_matrix(q)
new_matrix = np.eye(4)
new_matrix[:, 0] = matrix[:, 2]
new_matrix[:, 1] = -matrix[:, 1]
new_matrix[:, 2] = matrix[:, 0]
t_pre = t + new_matrix[0:3, 2] * offset_p
t_g = t + new_matrix[0:3, 2] * offset_g
quaternion_from_matrix
q = quaternion_from_matrix(new_matrix)
print("try grasp {0} to move to object...........Move it!".format(g))
while not rospy.is_shutdown():
i_pregrasp += 1
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = t_pre[0]
goal_1.pose.position.y = t_pre[1]
goal_1.pose.position.z = t_pre[2]
#Orientation as a quaternion
goal_1.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
plan = planner.plan_to_pose(goal_1, list())
## In Rviz
# rospy.sleep(6)
# raw_input("Press <Enter> to PRE grasp pose: ")
print("PRE grasp pose {0}, try {1} times".format(
g, i_pregrasp))
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
## jump to next grasp pose (1)
if i_pregrasp > 2:
try_another_grasp = 1
break
else:
break
## jump to next grasp pose (2)
if try_another_grasp == 1:
print('Try another !')
try_another_grasp = 0
planner.clear_goal()
continue
i_grasp = 0
while not rospy.is_shutdown():
i_grasp += 1
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = t_g[0]
goal_2.pose.position.y = t_g[1]
goal_2.pose.position.z = t_g[2]
#Orientation as a quaternion
goal_2.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
plan = planner.plan_to_pose(goal_2, list())
## in Rviz
# rospy.sleep(6)
# raw_input("Press <Enter> to GRASP pose : ")
print("GRASP pose %d" % i_grasp)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
##
## grasp
##
open_gripper(left)
close_gripper(left)
i_leave = 1
while not rospy.is_shutdown():
i_leave += 1
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
# [0.551, 0.690, -0.049]
goal_3.pose.position.x = 0.551
goal_3.pose.position.y = 0.690
goal_3.pose.position.z = -0.049
#Orientation as a quaternion
goal_3.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
plan = planner.plan_to_pose(goal_3, list())
# raw_input("Press <Enter> to LEAVE pose: ")
print("LEAVE pose %d" % i_leave)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
##
## grasp
##
open_gripper(left)
raw_input("Press <Enter> to continue")
# raise Exception("Execution failed")
# except Exception as e:
# print(e)
# else:
# break
while not rospy.is_shutdown():
raw_input("press enter to start")
move_to_default(planner)
# Move left arm to vision pose.
while not rospy.is_shutdown():
try:
plan = planner_left.plan_to_pose(vision_pose, [])
print(plan)
raw_input(
"Press <Enter> to move the left arm to vision state: ")
result = planner_left.execute_plan(plan)
print(result)
if not result:
raise Exception("Execution failed")
except Exception as e:
print(e)
else:
break
raw_input("Press enter if camera correctly positioned")
#Get positions of cubes.
def main():
"""
Main Script
"""
#############
offset = -0.3
#############
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("left_arm")
##
## Init Gripper
##
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
left = baxter_interface.Gripper('left', CHECK_VERSION)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 0.1;
# orien_const.weight = 1.0;
tf_listener = tf.TransformListener(rospy.Duration(1))
id_num = raw_input("Input Grasp ID: ")
tf_listener.waitForTransform('base', "sample_grasp_" + str(id_num),
rospy.Time(0), rospy.Duration(2.0))
t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(id_num),
rospy.Time())
matrix = quaternion_matrix(q)
new_matrix = matrix
# new_matrix = np.eye(4)
# new_matrix[:, 0] = matrix[:, 2]
# new_matrix[:, 1] = -matrix[:, 1]
# new_matrix[:, 2] = matrix[:, 0]
t_pre = t + new_matrix[0:3, 2] * offset
quaternion_from_matrix
q = quaternion_from_matrix(new_matrix)
print("try to move to object...........Move it!")
##########
id_num = 0
##########
while not rospy.is_shutdown():
i_pregrasp = 0
while not rospy.is_shutdown():
###########################################
# if i_pregrasp > 5:
# tf_listener.waitForTransform('base', "sample_grasp_" + str(id_num), rospy.Time(0), rospy.Duration(2.0))
# t, q = tf_listener.lookupTransform("base", "sample_grasp_" + str(id_num), rospy.Time())
# if id_num > rospy.get_param('good_grasps'):
# print("Fail!")
# return
############################################
i_pregrasp += 1
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = t_pre[0]
goal_1.pose.position.y = t_pre[1]
goal_1.pose.position.z = t_pre[2]
#Orientation as a quaternion
goal_1.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
plan = planner.plan_to_pose(goal_1, list())
print(plan)
if not plan:
raise Exception("Execution failed")
## in Rviz
rospy.sleep(6)
raw_input("Press <Enter> to PRE grasp pose: ")
print("PRE grasp pose %d" % i_pregrasp)
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
else:
break
i_grasp = 0
while not rospy.is_shutdown():
i_grasp += 1
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = t[0]
goal_2.pose.position.y = t[1]
goal_2.pose.position.z = t[2]
#Orientation as a quaternion
goal_2.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
plan = planner.plan_to_pose(goal_2, list())
if not plan:
raise Exception("Execution failed")
## in Rviz
rospy.sleep(6)
raw_input("Press <Enter> to GRASP pose : ")
print("GRASP pose %d" % i_grasp)
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
else:
break
##
## grasp
##
# open_gripper(left)
# close_gripper(left)
i_leave = 1
while not rospy.is_shutdown():
i_leave += 1
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
# [0.551, 0.690, -0.049]
goal_3.pose.position.x = 0.551
goal_3.pose.position.y = 0.690
goal_3.pose.position.z = -0.049
#Orientation as a quaternion
goal_3.pose.orientation = Quaternion(q[0], q[1], q[2], q[3])
plan = planner.plan_to_pose(goal_3, list())
if not plan:
raise Exception("Execution failed")
rospy.sleep(6)
raw_input("Press <Enter> to LEAVE pose: ")
print("LEAVE pose %d" % i_leave)
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
else:
break
##
## grasp
##
open_gripper(left)
raw_input("Press <Enter> to continue")
def main():
"""
Main Script
"""
#===================================================
# Code to add gripper
#rospy.init_node('gripper_test')
# # Set up the right gripper
#right_gripper = robot_gripper.Gripper('right_gripper')
# #Calibrate the gripper (other commands won't work unless you do this first)
#print('Calibrating...')
#right_gripper.calibrate()
#rospy.sleep(2.0)
# #Close the right gripper to hold publisher
# # MIGHT NOT NEED THIS
# print('Closing...')
# right_gripper.close()
# rospy.sleep(1.0)
#===================================================
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
#Tower
X = .075
Y = .15
Z = .0675
Xp = 0
Yp = 0
Zp = 0
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
# pose = PoseStamped()
# pose.header.stamp = rospy.Time.now()
# #TODO: Is this the correct frame name?
# pose.header.frame_id = "ar_marker_4"
# pose.pose.position.x = Xp
# pose.pose.position.y = Yp
# pose.pose.position.z = Zp
# pose.pose.orientation.x = Xpa
# pose.pose.orientation.y = Ypa
# pose.pose.orientation.z = Zpa
# pose.pose.orientation.w = Wpa
# planner.add_box_obstacle([X,Y,Z], "tower", pose)
#Table
X = 1
Y = 2
Z = .0675
Xp = 1.2
Yp = 0
Zp = -0.22
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
#TODO: Is this the correct frame name?
pose.header.frame_id = "base"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
planner.add_box_obstacle([X, Y, Z], "table", pose)
try:
#right_gripper_tip
goal_1 = PoseStamped()
goal_1.header.frame_id = "ar_marker_4"
#x, y, and z position
goal_1.pose.position.y = 0
goal_1.pose.position.z = .5
#removing the 6th layer
goal_1.pose.position.x = 0 #0.0825
#Orientation as a quaternion
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = 0
goal_1.pose.orientation.z = 0.707
goal_1.pose.orientation.w = 0.707
plan = planner.plan_to_pose(goal_1, list())
if not planner.execute_plan(plan):
raise Exception("Execution failed0")
except Exception as e:
print e
def main(cube_pose, end_pose):
planner = PathPlanner("right_arm")
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 10000;
orien_const.weight = 1.0;
# allow only 1/2 pi rotation so as to not lose the cube during transit
orien_path_const = OrientationConstraint()
orien_path_const.link_name = "right_gripper";
orien_path_const.header.frame_id = "base";
orien_path_const.orientation.y = -1.0;
orien_path_const.absolute_x_axis_tolerance = 0.1;
orien_path_const.absolute_y_axis_tolerance = 0.1;
orien_path_const.absolute_z_axis_tolerance = 1.57;
orien_path_const.weight = 1.0;
# quat = QuaternionStamped()
# quat.header.frame_id = "base"
# quat.quaternion.w = 1.0
start_pose = get_start(cube_pose, end_pose)
end_pose.pose.orientation = start_pose.pose.orientation
table_size = np.array([0.6, 1.2, 0.03])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
#x, y, and z position
table_pose.pose.position.x = 0.5
if ROBOT == "baxter":
table_pose.pose.position.z = -0.23 # for baxter
else:
table_pose.pose.position.z = -0.33
#Orientation as a quaternion
table_pose.pose.orientation.w = 1.0
planner.add_box_obstacle(table_size, "table", table_pose)
# cube_size = np.array([0.05, 0.05, 0.15])
default_state = get_pose(0.6, -0.5, -0.17, 0.0, -1.0, 0.0, 0.0)
# default_state = get_pose(0.94, 0.2, 0.5, 0.0, -1.0, 0.0, 0.0)
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(default_state, [])
print(type(plan))
raw_input("Press <Enter> to move the right arm to default pose: ")
if not planner.execute_plan(plan):
raise Exception("(ours) Execution failed")
except Exception as e:
print("caught!")
print e
else:
break
# planner.add_box_obstacle(cube_size, "cube", cube_pose)
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(start_pose, [orien_const])
raw_input("Press <Enter> to move the right arm to cube: ")
if not planner.execute_plan(plan):
raise Exception("(ours) Execution failed")
except Exception as e:
print("caught!")
print e
else:
break
orien_path_const.orientation = start_pose.pose.orientation
# planner.remove_obstacle("cube")
while not rospy.is_shutdown():
try:
plan = planner.plan_to_pose(end_pose, [orien_path_const])
raw_input("Press <Enter> to move the right arm to end position: ")
if not planner.execute_plan(plan):
raise Exception("(ours) Execution failed")
except Exception as e:
print("caught!")
print e
else:
break
def main():
planner = PathPlanner("right_arm")
#Wait for the IK service to become available
#rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
place_holder = {'images': [], 'camera_infos': []}
rospy.Subscriber("/cameras/head_camera/image", Image,
lambda x: place_holder['images'].append(x))
rospy.Subscriber("/cameras/head_camera/camera_info", CameraInfo,
lambda x: place_holder['camera_infos'].append(x))
limb = Limb("right")
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
while not rospy.is_shutdown() and count < 3:
#calibration points 1-3, closest to robot right = origin, closest to robot left, id_card 3 3/8 inches forward from first point
'''
/cameras/head_camera/camera_info
/cameras/head_camera/camera_info_std
/cameras/head_camera/image
'''
count += 1
raw_input("calibrate point " + str(count))
pos = limb.endpoint_pose()
calibration_points.append({})
calibration_points[-1]['world_coordinates'] = (pos['position'].x,
pos['position'].y,
pos['position'].z)
calibration_points[-1]['camera_info'] = place_holder['camera_infos'][
-1]
calibration_points[-1]['image'] = place_holder['images'][-1]
# for i in calibration_points:
# print(i['world_coordinates'])
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
## Add the controller here! IF you uncomment below, you should get the checkoff!
c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
for i in range(10):
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
x, y, z = .67, -.2, .108
else:
raise Exception("not configured for sawyer yet.")
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
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_1, [])
raw_input(
"Press <Enter> to move the right arm to goal pose 1: ")
start = time.time()
if not c.execute_path(plan):
raise Exception("Execution failed")
runtime = time.time() - start
with open(
'/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt',
'a') as f:
f.write(str(runtime) + ' move above point \n')
except Exception as e:
print e
traceback.print_exc()
else:
break
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
x, y, z = .67, -.2, 0
else:
raise Exception("not configured for sawyer yet.")
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = x
goal_2.pose.position.y = y
goal_2.pose.position.z = z
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_2, [])
raw_input(
"Press <Enter> to move the right arm to goal pose 2: ")
start = time.time()
if not c.execute_path(plan):
raise Exception("Execution failed")
runtime = time.time() - start
with open(
'/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt',
'a') as f:
f.write(str(runtime) + ' move to point \n')
except Exception as e:
print e
traceback.print_exc()
else:
break
def main():
"""
Examples of how to run me:
python scripts/main.py --help <------This prints out all the help messages
and describes what each parameter is
python scripts/main.py -t 1 -ar 1 -c workspace -a left --log
python scripts/main.py -t 2 -ar 2 -c velocity -a left --log
python scripts/main.py -t 3 -ar 3 -c torque -a right --log
python scripts/main.py -t 1 -ar 4 5 --path_only --log
NOTE: If running with the --moveit flag, it makes no sense
to also choose your controller to be workspace, since moveit
cannot accept workspace trajectories. This script simply ignores
the controller selection if you specify both --moveit and
--controller_name workspace, so if you want to run with moveit
simply leave --controller_name as default.
You can also change the rate, timeout if you want
"""
parser = argparse.ArgumentParser()
parser.add_argument('-task',
'-t',
type=str,
default='line',
help='Options: line, circle, square. Default: line')
parser.add_argument('-ar_marker',
'-ar',
nargs='+',
help='Which AR marker to use. Default: 1')
parser.add_argument(
'-controller_name',
'-c',
type=str,
default='jointspace',
help='Options: workspace, jointspace, or torque. Default: jointspace')
parser.add_argument('-arm',
'-a',
type=str,
default='left',
help='Options: left, right. Default: left')
parser.add_argument('-rate',
type=int,
default=200,
help="""
This specifies how many ms between loops. It is important to use a rate
and not a regular while loop because you want the loop to refresh at a
constant rate, otherwise you would have to tune your PD parameters if
the loop runs slower / faster. Default: 200""")
parser.add_argument(
'-timeout',
type=int,
default=None,
help=
"""after how many seconds should the controller terminate if it hasn\'t already.
Default: None""")
parser.add_argument(
'-num_way',
type=int,
default=300,
help=
'How many waypoints for the :obj:`moveit_msgs.msg.RobotTrajectory`. Default: 300'
)
parser.add_argument(
'--moveit',
action='store_true',
help=
"""If you set this flag, moveit will take the path you plan and execute it on
the real robot""")
parser.add_argument('--log',
action='store_true',
help='plots controller performance')
args = parser.parse_args()
rospy.init_node('moveit_node')
# this is used for sending commands (velocity, torque, etc) to the robot
limb = baxter_interface.Limb(args.arm)
# this is used to get the dynamics (inertia matrix, manipulator jacobian, etc) from the robot
# in the current position, UNLESS you specify other joint angles. see the source code
# https://github.com/valmik/baxter_pykdl/blob/master/src/baxter_pykdl/baxter_pykdl.py
# for info on how to use each method
kin = baxter_kinematics(args.arm)
# ADD COMMENT EHRE
tag_pos = [lookup_tag(marker) for marker in args.ar_marker]
# Get an appropriate RobotTrajectory for the task (circular, linear, or square)
# If the controller is a workspace controller, this should return a trajectory where the
# positions and velocities are workspace positions and velocities. If the controller
# is a jointspace or torque controller, it should return a trajectory where the positions
# and velocities are the positions and velocities of each joint.
robot_trajectory = get_trajectory(args.task, limb, kin, tag_pos,
args.num_way, args.controller_name)
# This is a wrapper around MoveIt! for you to use. We use MoveIt! to go to the start position
# of the trajectory
planner = PathPlanner('{}_arm'.format(args.arm))
if args.controller_name == "workspace":
pose = create_pose_stamped_from_pos_quat(
robot_trajectory.joint_trajectory.points[0].positions,
[0, 1, 0, 0], 'base')
plan = planner.plan_to_pose(pose)
else:
plan = planner.plan_to_joint_pos(
robot_trajectory.joint_trajectory.points[0].positions)
planner.execute_plan(plan)
if args.moveit:
# LAB 1 PART A
# by publishing the trajectory to the move_group/display_planned_path topic, you should
# be able to view it in RViz. You will have to click the "loop animation" setting in
# the planned path section of MoveIt! in the menu on the left side of the screen.
pub = rospy.Publisher('move_group/display_planned_path',
DisplayTrajectory,
queue_size=10)
disp_traj = DisplayTrajectory()
disp_traj.trajectory.append(robot_trajectory)
# disp_traj.trajectory_start = planner._group.get_current_joint_values()
disp_traj.trajectory_start = RobotState()
pub.publish(disp_traj)
try:
raw_input('Press <Enter> to execute the trajectory using MOVEIT')
except KeyboardInterrupt:
sys.exit()
# uses MoveIt! to execute the trajectory. make sure to view it in RViz before running this.
# the lines above will display the trajectory in RViz
planner.execute_plan(robot_trajectory)
else:
# LAB 1 PART B
controller = get_controller(args.controller_name, limb, kin)
try:
raw_input(
'Press <Enter> to execute the trajectory using YOUR OWN controller'
)
except KeyboardInterrupt:
sys.exit()
# execute the path using your own controller.
done = controller.execute_path(robot_trajectory,
rate=args.rate,
timeout=args.timeout,
log=args.log)
if not done:
print 'Failed to move to position'
sys.exit(0)
def actuate(actual, goals):
planner = PathPlanner("right_arm")
gripper = robot_gripper.Gripper('right')
#Calibrate the gripper (other commands won't work unless you do this first)
gripper.clear_calibration()
gripper.calibrate()
rospy.sleep(2.0)
# ##
# ## Add the obstacle to the planning scene here
### SHOULD WE ADD PLACED PIECES AS OBSTACLES ????
##
# pose = PoseStamped()
# pose.header.frame_id = "base"
# pose.pose.position.x = .5
# pose.pose.position.y = 0.0
# pose.pose.position.z = -.2
# 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(np.ndarray(shape=(3,), buffer=np.array([.4, 1.2, .1])), "table", pose)
# pose = PoseStamped()
# pose.header.frame_id = "base"
# pose.pose.position.x = -.5
# pose.pose.position.y = 0.0
# pose.pose.position.z = 0.0
# 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(np.ndarray(shape=(3,), buffer=np.array([.1, 3, 3])), "wall", pose)
# #Create a path constraint for the arm
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 0.1;
# orien_const.weight = 1.0;
# List of letters corresponding to the pieces in the actual configuration
actual_list = actual.keys()
print(actual_list)
# Creation of a copy PoseStamped message to use for z translation
above = PoseStamped()
above.header.frame_id = "base"
raw_input("Press <Enter> to move the right arm to pick up a piece: ")
while not rospy.is_shutdown() and len(actual_list) > 0:
# Booleans to track state
in_hand, picked, placed = False, False, False
piece = actual_list[0]
### PICKING UP PIECE FROM ACTUAL LOCATION ###
while not rospy.is_shutdown() and not picked:
print("PICKING")
try:
original = actual[piece]
print(original)
# Setting above's value to correspond to just above original position
above.pose.position.x = original.pose.position.x
above.pose.position.y = original.pose.position.y
above.pose.position.z = original.pose.position.z + 0.2
above.pose.orientation = original.pose.orientation
# Pick up the piece in the original position
if not in_hand:
# Translate over such that the piece is above the actual position
plan = planner.plan_to_pose(above, [])
if not planner.execute_plan(plan):
raise Exception("Execution failed")
# Actually pick up the piece
plan = planner.plan_to_pose(original, [])
if not planner.execute_plan(plan):
raise Exception("Execution failed")
## GRIPPER CLOSE (succ)
gripper.close()
rospy.sleep(2.0)
in_hand = True
# Raise the arm a little bit so that other pieces are not affected
plan = planner.plan_to_pose(above, [])
if not planner.execute_plan(plan):
raise Exception("Execution failed")
### PLACING PIECE AT DESIRED LOCATION ###
text = raw_input("Press <Enter> to move the right arm to place the piece or 'back' to redo the previous step: ")
if text == "back":
picked = False
else:
picked = True
except Exception as e:
print e
traceback.print_exc()
else:
break
while not rospy.is_shutdown() and picked and not placed:
print("PLACING")
try:
goal = goals.get(piece)[0]
print(goal)
# Setting above's value to correspond to just above original position
above.pose.position.x = goal.pose.position.x
above.pose.position.y = goal.pose.position.y
above.pose.position.z = goal.pose.position.z + 0.2
above.pose.orientation = goal.pose.orientation
if in_hand:
# Translate over such that the piece is above the desired position
plan = planner.plan_to_pose(above, [])
if not planner.execute_plan(plan):
raise Exception("Execution failed")
# Actually place the piece on table
plan = planner.plan_to_pose(goal, [])
if not planner.execute_plan(plan):
raise Exception("Execution failed")
## GRIPPER OPEN (release)
gripper.open()
rospy.sleep(2.0)
in_hand = False
# Raise the arm a little bit so that other pieces are not affected
plan = planner.plan_to_pose(above, [])
if not planner.execute_plan(plan):
raise Exception("Execution failed")
text = raw_input("Press <Enter> to move onto the next piece or 'back': ")
if text == "back":
placed = False
picked = True
else:
placed = True
picked = False
goals.get(piece).pop(0)
actual_list.pop(0)
except Exception as e:
print e
traceback.print_exc()
else:
break
class Mover:
def __init__(self, box_in):
self.box = box_in
self.conveyor_z = -0.038
self.mover_setup()
def mover_setup(self):
limb = Limb("right")
# Right arm planner
self.planner = PathPlanner("right_arm")
# Left arm planner
self.planner_left = PathPlanner("left_arm")
place_holder = {'images': [], 'camera_infos': []}
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
# Controller for right arm
self.c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
self.orien_const = OrientationConstraint()
self.orien_const.link_name = "right_gripper";
self.orien_const.header.frame_id = "base";
self.orien_const.orientation.y = -1.0;
self.orien_const.absolute_x_axis_tolerance = 0.1;
self.orien_const.absolute_y_axis_tolerance = 0.1;
self.orien_const.absolute_z_axis_tolerance = 0.1;
self.orien_const.weight = 1.0;
box = PoseStamped()
box.header.frame_id = "base"
box.pose.position.x = self.box.pose.position.x
box.pose.position.y = self.box.pose.position.y
# box.pose.position.z = self.box.pose.position.z
box.pose.position.z = self.conveyor_z
# box.pose.position.x = 0.5
# box.pose.position.y = 0.0
# box.pose.position.z = 0.0
box.pose.orientation.x = 0.0
box.pose.orientation.y = 0.0
box.pose.orientation.z = 0.0
box.pose.orientation.w = 1.0
self.planner.add_box_obstacle((100, 100, 0.00001), "box", box)
# Controller for left arm
self.c_left = Controller(Kp, Ki, Kd, Kw, Limb('left'))
def move(self, x, y, z):
try:
print("Moving\n\n\n")
print(x, y, z)
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
#Orientation as a quaternion
goal.pose.orientation.x = 0.0
goal.pose.orientation.y = -1.0
goal.pose.orientation.z = 0.0
goal.pose.orientation.w = 0.0
plan = self.planner.plan_to_pose(goal, [self.orien_const])
# TODO: Remove the raw_input for the demo.
# x = raw_input("Move?")
if not self.c.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print(e)
traceback.print_exc()
def delayed_move(self, x, y, z, delay, time1):
try:
print("Delayed Move\n\n")
print(x, y, z)
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = x
goal.pose.position.y = y
goal.pose.position.z = z
#Orientation as a quaternion
goal.pose.orientation.x = 0.0
goal.pose.orientation.y = -1.0
goal.pose.orientation.z = 0.0
goal.pose.orientation.w = 0.0
plan = self.planner.plan_to_pose(goal, [self.orien_const])
# Wait till delay has elapsed.
while rospy.Time.now().to_sec() < time1 + delay:
pass
if not self.c.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print(e)
traceback.print_exc()
# Move camera to set position (optional if cannot find a good intermediary pose)
def move_camera(self):
try:
goal = PoseStamped()
goal.header.frame_id = "base"
# TODO: Update position and orientation values
#x, y, and z position
goal.pose.position.x = .655
goal.pose.position.y = .21
goal.pose.position.z = .25
#Orientation as a quaternion
goal.pose.orientation.x = -.7
goal.pose.orientation.y = .71
goal.pose.orientation.z = -.03
goal.pose.orientation.w = -.02
plan = self.planner.plan_to_pose(goal)
# TODO: Remove the raw_input for the demo.
x = raw_input("Move?")
if not self.c.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print(e)
traceback.print_exc()
class Actuator(object):
def __init__(self, sub_topic, pub_topic):
self.planner = None
self.limb = None
self.executed = False
self.planner = PathPlanner("right_arm")
self.limb = Limb("right")
# self.orien_const = OrientationConstraint()
# self.orien_const.link_name = "right_gripper"
# self.orien_const.header.frame_id = "base"
# self.orien_const.orientation.z = 0
# self.orien_const.orientation.y = 1
# self.orien_const.orientation.w = 0
# self.orien_const.absolute_x_axis_tolerance = 0.1
# self.orien_const.absolute_y_axis_tolerance = 0.1
# self.orien_const.absolute_z_axis_tolerance = 0.1
# self.orien_const.weight = 1.0
size = [1, 2, 2]
obstacle_pose = PoseStamped()
obstacle_pose.header.frame_id = "base"
obstacle_pose.pose.position.x = -1
obstacle_pose.pose.position.y = 0
obstacle_pose.pose.position.z = 0
obstacle_pose.pose.orientation.x = 0
obstacle_pose.pose.orientation.y = 0
obstacle_pose.pose.orientation.z = 0
obstacle_pose.pose.orientation.w = 1
self.planner.add_box_obstacle(size, "wall", obstacle_pose)
size = [.75, 1, 1]
obstacle_pose = PoseStamped()
obstacle_pose.header.frame_id = "base"
obstacle_pose.pose.position.x = 1
obstacle_pose.pose.position.y = 0
obstacle_pose.pose.position.z = -.4
obstacle_pose.pose.orientation.x = 0
obstacle_pose.pose.orientation.y = 0
obstacle_pose.pose.orientation.z = 0
obstacle_pose.pose.orientation.w = 1
self.planner.add_box_obstacle(size, "table", obstacle_pose)
self.pub = rospy.Publisher(pub_topic, Bool)
self.sub = rospy.Subscriber(sub_topic, PoseStamped, self.callback)
def callback(self, goal):
while not self.executed:
# if goal.pose.position.y <= 0:
# self.planner = PathPlanner("right_arm")
# self.limb = Limb("right")
# else:
# self.planner = PathPlanner("left_arm")
# self.limb = Limb("left")
try:
plan = self.planner.plan_to_pose(goal, [])
raw_input("Press <Enter> to execute plan")
if not self.planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
self.executed = True
print("Execution succeeded! Release the ball when ready!")
break
self.pub.publish(True)
# If the controller is a workspace controller, this should return a trajectory where the
# positions and velocities are workspace positions and velocities. If the controller
# is a jointspace or torque controller, it should return a trajectory where the positions
# and velocities are the positions and velocities of each joint.
robot_trajectory = get_trajectory(args.task, current_pos, tag_pos,
args.num_way, args.controller_name, limb,
kin, args.rate)
# This is a wrapper around MoveIt! for you to use. We use MoveIt! to go to the start position
# of the trajectory
planner = PathPlanner('{}_arm'.format(args.arm))
if args.controller_name == "workspace":
pose = create_pose_stamped_from_pos_quat(
robot_trajectory.joint_trajectory.points[0].positions,
[0, 1, 0, 0], 'base')
plan = planner.plan_to_pose(pose)
else:
plan = planner.plan_to_joint_pos(
robot_trajectory.joint_trajectory.points[0].positions)
planner.execute_plan(plan)
if args.moveit:
# LAB 1 PART A
# by publishing the trajectory to the move_group/display_planned_path topic, you should
# be able to view it in RViz. You will have to click the "loop animation" setting in
# the planned path section of MoveIt! in the menu on the left side of the screen.
pub = rospy.Publisher('move_group/display_planned_path',
DisplayTrajectory,
queue_size=10)
disp_traj = DisplayTrajectory()
disp_traj.trajectory.append(robot_trajectory)
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
##
## Add the obstacle to the planning scene here
##
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
# obs1 = PoseStamped()
# obs1.pose.position.x = 0.5
# obs1.pose.position.y = 0.0
# obs1.pose.position.z = -0.3
# obs1.pose.orientation.x = 0.0
# obs1.pose.orientation.y = 0.0
# obs1.pose.orientation.z = 0.0
# obs1.pose.orientation.w = 1.0
# obs1.header.frame_id = "base"
# planner.add_box_obstacle([0.4, 1.2, 0.1], 'box', obs1)
# obs2 = PoseStamped()
# obs2.pose.position.x = 0.5
# obs2.pose.position.y = 0.0
# obs2.pose.position.z = -0.25
# obs2.pose.orientation.x = 1.0
# obs2.pose.orientation.y = 1.0
# obs2.pose.orientation.z = 1.0
# obs2.pose.orientation.w = 1.0
# obs2.header.frame_id = "base"
# planner.add_box_obstacle([0.4, 1.2, 0.1], 'box', obs2)
raw_input("Press <Enter> to move the right arm to goal pose 1: ")
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.68
goal_1.pose.position.y = 0.73
goal_1.pose.position.z = -0.03
#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
cont = controller.Controller(Kp, Kd, Ki, Kw, Limb('right'))
plan = planner.plan_to_pose(goal_1, list())
if not cont.execute_path(plan):
# if not planner.execute_plan(plan):
raise ZeroDivisionError("Execution failed")
except ZeroDivisionError as e:
raise e
else:
break
def draw(top, bot, board):
Q = homogenous(top.T, board)
top = np.dot(Q, top.T).T
bot = np.dot(Q, bot.T).T
planner = PathPlanner('right_arm')
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
controller = Controller(Kp, Kd, Ki, Kw, Limb('right'))
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
ori = PoseStamped()
ori.header.frame_id = 'base'
ori.pose.orientation.x = 0.0
ori.pose.orientation.y = -1.0
ori.pose.orientation.z = 0.0
ori.pose.orientation.w = 0.0
goals = np.empty(len(top) + len(bot), dtype=object)
while not rospy.is_shutdown():
try:
counter = 0
count_top = 0
count_bot = len(bot) - 1
while count_top < len(top) or count_bot >= 0:
if count_top < len(top):
goal = PoseStamped()
goal.header.frame_id = "base"
goal.pose.position.x = top[count_top][0] / top[count_top][
2] - 0.014 + .012
goal.pose.position.y = top[count_top][1] / top[count_top][
2] + 0.005 - 0.04
goal.pose.position.z = -0.1849
goals[counter] = goal
counter += 1
count_top += 1
if count_bot >= 0:
goal = PoseStamped()
goal.header.frame_id = "base"
goal.pose.position.x = bot[count_bot][0] / bot[count_bot][
2] + 0.003
goal.pose.position.y = bot[count_bot][1] / bot[count_bot][
2] - 0.01
goal.pose.position.z = -0.1861
goals[counter] = goal
counter += 1
count_bot -= 1
for i in goals:
i.pose.orientation = ori.pose.orientation
plan = planner.plan_to_pose(i, list())
if not controller.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
planner = PathPlanner("left_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# #Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
filename = "test_cup.jpg"
table_height = 0
# get positions of cups
cups = find_cups(filename)
start_cup = cups[0]
goal_cup = cups[1]
cup_pos = [0.756, 0.300, -0.162] # position of cup
goal_pos = [0.756, 0.600, -0.162] # position of goal cup
init_pos = [cup_pos[0] - .2, cup_pos[1],
cup_pos[2] + .05] # initial position
start_pos = [init_pos[0] + .2, init_pos[1],
init_pos[2]] # start position, pick up cup
up_pos = [goal_pos[0], goal_pos[1] - .05,
goal_pos[2] + .1] # up position, ready to tilt
end_pos = start_pos # end position, put down cup
final_pos = init_pos # final position, away from cup
# add table obstacle
table_size = np.array([.4, .8, .1])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
#x, y, and z position
table_pose.pose.position.x = 0.5
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = 0.0
planner.add_box_obstacle(table_size, 'table', table_pose)
# add goal cup obstacle
goal_cup_size = np.array([.1, .1, .2])
goal_cup_pose = PoseStamped()
goal_cup_pose.header.frame_id = "base"
#x, y, and z position
goal_cup_pose.pose.position.x = goal_pos[0]
goal_cup_pose.pose.position.y = goal_pos[1]
goal_cup_pose.pose.position.z = goal_pos[2]
planner.add_box_obstacle(goal_cup_size, 'goal cup', goal_cup_pose)
# add first cup obstacle
cup_size = np.array([.1, .1, .2])
cup_pose = PoseStamped()
cup_pose.header.frame_id = "base"
#x, y, and z position
cup_pose.pose.position.x = cup_pos[0]
cup_pose.pose.position.y = cup_pos[1]
cup_pose.pose.position.z = cup_pos[2]
planner.add_box_obstacle(cup_size, 'cup', cup_pose)
while not rospy.is_shutdown():
left_gripper = robot_gripper.Gripper('left')
print('Calibrating...')
left_gripper.calibrate()
pose = PoseStamped()
pose.header.frame_id = "base"
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = -1.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = init_pos[0]
pose.pose.position.y = init_pos[1]
pose.pose.position.z = init_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to initial pose: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# grab cup
# need to make sure it doesn't knock it over
planner.remove_obstacle('cup')
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = start_pos[0]
pose.pose.position.y = start_pos[1]
pose.pose.position.z = start_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to grab the cup: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
print('Closing...')
left_gripper.close()
rospy.sleep(2)
except Exception as e:
print e
else:
break
# position to pour
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = up_pos[0]
pose.pose.position.y = up_pos[1]
pose.pose.position.z = up_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to above the goal cup: "
)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# pouring
raw_input("Press <Enter> to move the left arm to begin pouring: ")
for degree in range(180):
while not rospy.is_shutdown():
try:
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = -1.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 0.0
plan = planner.plan_to_pose(pose, [])
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# move cup away from goal on table
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = end_pos[0]
pose.pose.position.y = end_pos[1]
pose.pose.position.z = end_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to away from the goal cup: "
)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# let go of cup on table
# need to make sure to not to hit cup
while not rospy.is_shutdown():
try:
#x, y, and z position
pose.pose.position.x = final_pos[0]
pose.pose.position.y = final_pos[1]
pose.pose.position.z = final_pos[2]
plan = planner.plan_to_pose(pose, [orien_const])
raw_input(
"Press <Enter> to move the left arm to let go of the cup: "
)
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
# get new cup position
new_cup_pos = []
# readd the cup obstacle
cup_pose.pose.position.x = new_cup_pos[0]
cup_pose.pose.position.y = new_cup_pos[1]
cup_pose.pose.position.z = new_cup_pos[2]
planner.add_box_obstacle(cup_size, 'cup', cup_pose)
def move():
planner = PathPlanner('right_arm')
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
controller = Controller(Kp, Kd, Ki, Kw, Limb('right'))
ori = PoseStamped()
ori.header.frame_id = 'base'
ori.pose.orientation.x = 0.0
ori.pose.orientation.y = -1.0
ori.pose.orientation.z = 0.0
ori.pose.orientation.w = 0.0
diff = 0
observe(150000)
ready(20000)
#draw(planner, controller, ori)
while not rospy.is_shutdown():
for _ in range(10):
while not rospy.is_shutdown():
try:
#rosrun tf tf_echo base right_gripper_tip
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.65
goal_1.pose.position.y = 0.02 + diff
goal_1.pose.position.z = -0.227
#Orientation as a quaternion
goal_1.pose.orientation = ori.pose.orientation
plan1 = planner.plan_to_pose(goal_1, list())
if not controller.execute_path(plan1):
raise Exception("Execution failed")
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.80
goal_2.pose.position.y = -0.01 + diff
goal_2.pose.position.z = -0.227 #should be a little bit lower than initial point
#Orientation as a quaternion
goal_2.pose.orientation = ori.pose.orientation
plan2 = planner.plan_to_pose(goal_2, list())
diff += .004
#raw_input("Press <Enter>")
if not controller.execute_path(plan2):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
observe(50000)
ready(20000)
def main():
"""
Main Script
"""
use_orien_const = False
input = raw_input("use orientation constraint? y/n\n")
if input == 'y' or input == 'yes':
use_orien_const = True
print("using orientation constraint")
elif input == 'n' or input == 'no':
use_orien_const = False
print("not using orientation constraint")
else:
print("input error, not using orientation constraint as default")
add_box = False
input = raw_input("add_box? y/n\n")
if input == 'y' or input == 'yes':
add_box = True
print("add_box")
elif input == 'n' or input == 'no':
add_box = False
print("not add_box")
else:
print("input error, not add_box as default")
open_loop_contro = False
input = raw_input("using open loop controller? y/n\n")
if input == 'y' or input == 'yes':
open_loop_contro = True
print("using open loop controller")
elif input == 'n' or input == 'no':
open_loop_contro = False
print("not using open loop controller")
else:
print("input error, not using open loop controller as default")
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# joint_names = ['head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
my_controller = Controller(Kp, Kd, Ki, Kw, Limb("right"))
##
## Add the obstacle to the planning scene here
##
if add_box:
name = "obstacle_1"
size = np.array([0.4, 1.2, 0.1])
pose = PoseStamped()
pose.header.frame_id = "base"
#x, y, and z position
pose.pose.position.x = 0.5
pose.pose.position.y = 0.0
pose.pose.position.z = 0.0
#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)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.4
goal_1.pose.position.y = -0.3
goal_1.pose.position.z = 0.2
#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
if not use_orien_const:
plan = planner.plan_to_pose(goal_1, list())
else:
plan = planner.plan_to_pose(goal_1, [orien_const])
raw_input(
"Press <Enter> to move the right arm to goal pose 1: ")
if open_loop_contro:
if not my_controller.execute_path(plan):
raise Exception("Execution failed")
else:
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
# plan = planner.plan_to_pose(goal_2, list())
if not use_orien_const:
plan = planner.plan_to_pose(goal_2, list())
else:
plan = planner.plan_to_pose(goal_2, [orien_const])
raw_input(
"Press <Enter> to move the right arm to goal pose 2: ")
if open_loop_contro:
if not my_controller.execute_path(plan):
raise Exception("Execution failed")
else:
if not planner.execute_plan(plan):
raise Exception("Execution failed")
# if not my_controller.execute_path(plan):
# raise Exception("Execution failed")
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
# plan = planner.plan_to_pose(goal_3, list())
if not use_orien_const:
plan = planner.plan_to_pose(goal_3, list())
else:
plan = planner.plan_to_pose(goal_3, [orien_const])
raw_input(
"Press <Enter> to move the right arm to goal pose 3: ")
if open_loop_contro:
if not my_controller.execute_path(plan):
raise Exception("Execution failed")
else:
if not planner.execute_plan(plan):
raise Exception("Execution failed")
# if not my_controller.execute_path(plan):
# raise Exception("Execution failed")
# if not planner.execute_plan(plan):
# raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
##
## Add the obstacle to the planning scene here
##
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 0.1;
# orien_const.weight = 1.0;
#hardcodeDESE COORDINATE VALUES
#IN THE VIEW OF THE CAMERA
#CORNER1--------->ORNER2
# | |
# | |
# | |
#CORNER3 ------------|
CORNER1 =
CORNER2 =
CORNER3 =
#CREATE THE GRID
dir1 = CORNER2 - CORNER1
dir2 = CONRER3 - CORNER1
grid_vals = []
for i in range(0, 4):
for j in range(0, 4):
grid = CORNER1 + i * dir1 / 3 + j * dir2 / 3
grid_vals.append(grid)
while not rospy.is_shutdown():
for g in grid_vals:
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = g[0]
goal_1.pose.position.y = g[1]
goal_1.pose.position.z = g[2]
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = 0.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 1
plan = planner.plan_to_pose(goal_1, list())
raw_input("Press <Enter> to move the right arm to goal: " + "x = " + str(g[0]) + "y = "
+ str(g[1]) + " z = " + str(g[2]))
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main(list_of_class_objs, basket_coordinates, planner_left):
"""
Main Script
input:first argument: a list of classX_objs. Ex: [class1_objs, class2_objs] where class1_objs contains the same kind of fruits
second argument: a list of baskets coordinates
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner_left = PathPlanner("left_arm")
home_coordinates = [0.544, 0.306, 0.3]
home = PoseStamped()
home.header.frame_id = "base"
#home x,y,z position
home_x = home_coordinates[0]
home_y = home_coordinates[1]
home_z = home_coordinates[2]
home.pose.position.x = home_x
home.pose.position.y = home_y
home.pose.position.z = home_z
#Orientation as a quaternion
home.pose.orientation.x = 1.0
home.pose.orientation.y = 0.0
home.pose.orientation.z = 0.0
home.pose.orientation.w = 0.0
intermediate_obstacle = PoseStamped()
intermediate_obstacle.header.frame_id = "base"
intermediate_obstacle.pose.position.x = 0
intermediate_obstacle.pose.position.y = 0
intermediate_obstacle.pose.position.z = 0.764
intermediate_obstacle.pose.orientation.x = 1
intermediate_obstacle.pose.orientation.y = 0
intermediate_obstacle.pose.orientation.z = 0
intermediate_obstacle.pose.orientation.w = 0
intermediate_size = [1, 1, 0.2]
left_gripper = robot_gripper.Gripper('left')
print('Calibrating...')
left_gripper.calibrate()
while not rospy.is_shutdown():
try:
#GO HOME
plan = planner_left.plan_to_pose(home, list())
raw_input("Press <Enter> to move the left arm to home position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
for i, classi_objs in enumerate(list_of_class_objs):
#x, y,z, orientation of class(basket)
print("processing class: " + str(i))
classi_position = basket_coordinates[i]
classi = PoseStamped()
classi.header.frame_id = "base"
classi_x = classi_position[0]
classi_y = classi_position[1]
classi_z = classi_position[2]
classi.pose.position.x = classi_x
classi.pose.position.y = classi_y
classi.pose.position.z = classi_z +.1
classi.pose.orientation.x = 1.0
classi.pose.orientation.y = 0.0
classi.pose.orientation.z = 0.0
classi.pose.orientation.w = 0.0
for fruit in classi_objs:
gripper_yaw = fruit[3]
fruit_x = fruit[0]
fruit_y = fruit[1]
fruit_z = fruit[2]
gripper_orientation = euler_to_quaternion(gripper_yaw)
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
gripper_orientation_x = gripper_orientation[0]
gripper_orientation_y = gripper_orientation[1]
gripper_orientation_z = gripper_orientation[2]
gripper_orientation_w = gripper_orientation[3]
orien_const.orientation.x = gripper_orientation_x
orien_const.orientation.y = gripper_orientation_y
orien_const.orientation.z = gripper_orientation_z
orien_const.orientation.w = gripper_orientation_w
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose)
#intermidiate_to_fruit stage: move to the top of the fruit location and open the gripper
intermidiate_to_fruit = PoseStamped()
intermidiate_to_fruit.header.frame_id = "base"
#x,y,z position
intermidiate_to_fruit.pose.position.x = fruit_x
intermidiate_to_fruit.pose.position.y = fruit_y
intermidiate_to_fruit.pose.position.z = home_z - .1
print("Trying to reach intermeidiate_to_fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z))
intermidiate_to_fruit.pose.orientation.x = gripper_orientation_x
intermidiate_to_fruit.pose.orientation.y = gripper_orientation_y
intermidiate_to_fruit.pose.orientation.z = gripper_orientation_z
intermidiate_to_fruit.pose.orientation.w = gripper_orientation_w
plan = planner_left.plan_to_pose(intermidiate_to_fruit, list())
raw_input("Press <Enter> to move the left arm to intermidiate_to_fruit position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
planner_left.remove_obstacle("intermediate")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
#go down to the actual height of the fruit and close gripper
fruitobs = generate_obstacle(fruit_x, fruit_y)
fruit = PoseStamped()
fruit.header.frame_id = "base"
#x,y,z position
fruit.pose.position.x = fruit_x
fruit.pose.position.y = fruit_y
fruit.pose.position.z = fruit_z
print("Trying to reach fruit position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(fruit_z))
#Orientation as a quaternion
fruit.pose.orientation.x = gripper_orientation_x
fruit.pose.orientation.y = gripper_orientation_y
fruit.pose.orientation.z = gripper_orientation_z
fruit.pose.orientation.w = gripper_orientation_w
plan = planner_left.plan_to_pose(fruit, [orien_const])
raw_input("Press <Enter> to move the left arm to fruit position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
fruitobs()
except Exception as e:
print e
else:
break
#close the gripper
print('Closing...')
left_gripper.close()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
fruitobs = generate_obstacle(fruit_x, fruit_y)
#intermidiate_to_basket stage1: Lift up to a height higher than the height of the basket
firt_intermidiate_to_class_i = PoseStamped()
firt_intermidiate_to_class_i.header.frame_id = "base"
#x, y, and z position
firt_intermidiate_to_class_i.pose.position.x = fruit_x
firt_intermidiate_to_class_i.pose.position.y = fruit_y
firt_intermidiate_to_class_i.pose.position.z = classi_z + 0.25
print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2))
#Orientation as a quaternion
firt_intermidiate_to_class_i.pose.orientation.x = 1.0
firt_intermidiate_to_class_i.pose.orientation.y = 0.0
firt_intermidiate_to_class_i.pose.orientation.z = 0.0
firt_intermidiate_to_class_i.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(firt_intermidiate_to_class_i, list())
raw_input("Press <Enter> to move the left arm to first_intermidiate_to_class_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
fruitobs()
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
planner_left.add_box_obstacle(intermediate_obstacle, "intermediate", table_pose)
#intermidiate_to_basket stage2: Move to the top of the basket
intermidiate_to_class_i = PoseStamped()
intermidiate_to_class_i.header.frame_id = "base"
#x, y, and z position
intermidiate_to_class_i.pose.position.x = classi_x
intermidiate_to_class_i.pose.position.y = classi_y
intermidiate_to_class_i.pose.position.z = classi_z + 0.2
print("Trying to reach intermidiate_to_class_i position :" + str(fruit_x) + " " + str(fruit_y) + " " + str(classi_position[2] + 0.2))
#Orientation as a quaternion
intermidiate_to_class_i.pose.orientation.x = 1.0
intermidiate_to_class_i.pose.orientation.y = 0.0
intermidiate_to_class_i.pose.orientation.z = 0.0
intermidiate_to_class_i.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(intermidiate_to_class_i, list())
raw_input("Press <Enter> to move the left arm to second_intermidiate_to_class_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
planner_left.remove_obstacle("intermediate")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
#basket stage: put the fruit in the basket
classi_obs = generate_obstacle(classi_x, class_y)
plan = planner_left.plan_to_pose(classi, list())
raw_input("Press <Enter> to move the left arm to sclass_" + str(i) + "position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
classi_obs()
except Exception as e:
print e
else:
break
#Open the gripper
print('Opening...')
left_gripper.open()
rospy.sleep(1.0)
while not rospy.is_shutdown():
try:
#intermidiate to home stage: lifting up to the home position height
intermidiate_to_home_1 = PoseStamped()
intermidiate_to_home_1.header.frame_id = "base"
#x, y, and z position
intermidiate_to_home_1.pose.position.x = classi_x
intermidiate_to_home_1.pose.position.y = classi_y
intermidiate_to_home_1.pose.position.z = home_z
#Orientation as a quaternion
intermidiate_to_home_1.pose.orientation.x = 1.0
intermidiate_to_home_1.pose.orientation.y = 0.0
intermidiate_to_home_1.pose.orientation.z = 0.0
intermidiate_to_home_1.pose.orientation.w = 0.0
plan = planner_left.plan_to_pose(intermidiate_to_home_1, list())
raw_input("Press <Enter> to move the right arm to intermidiate_to_home_1 position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
planner_left.shutdown()
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
controller = Controller(Kp, Kd, Ki, Kw, Limb('right'))
##
## Add the obstacle to the planning scene here
bagel = PoseStamped()
bagel.header.frame_id = 'base'
#x, y, and z position
bagel.pose.position.x = 0.5
bagel.pose.position.y = 0.0
bagel.pose.position.z = 0.0
#Orientation as a quaternion
bagel.pose.orientation.x = 0.0
bagel.pose.orientation.y = 0.0
bagel.pose.orientation.z = 0.0
bagel.pose.orientation.w = 1.0
planner.add_box_obstacle(np.array([0.4, 1.2, 0.1]), 'bagel', bagel)
#Create a path constraint for the arm
#UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.4
goal_1.pose.position.y = -0.3
goal_1.pose.position.z = 0.2
#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
plan = planner.plan_to_pose(goal_1, list())
raw_input("Press <Enter> to move the right arm to goal pose 1: ")
if not controller.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_2, list())
raw_input("Press <Enter> to move the right arm to goal pose 2: ")
if not controller.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_3, list())
raw_input("Press <Enter> to move the right arm to goal pose 3: ")
if not controller.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
X = 0.40
Y = 1.20
Z = 0.10
Xp = 0.5
Yp = 0.00
Zp = -0.15
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
#planner.add_box_obstacle([X,Y,Z], "wall", pose)
#Wall 2
X = 0.40
Y = 0.10
Z = 0.30
Xp = 0.5
Yp = -0.15
Zp = 0.05
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
#planner.add_box_obstacle([X,Y,Z], "wall2", pose)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.4
goal_1.pose.position.y = -0.3
goal_1.pose.position.z = 0.2
#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
plan = planner.plan_to_pose(goal_1, list())
raw_input("Press <Enter> to move the right arm to goal pose 1: ")
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_2, [orien_const])
raw_input("Press <Enter> to move the right arm to goal pose 2: ")
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_3, [orien_const])
raw_input("Press <Enter> to move the right arm to goal pose 3: ")
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
calib1 = PoseStamped()
calib1.header.frame_id = "base"
#x,y,z position
calib1.pose.position.x = 0.35
calib1.pose.position.y = 0.75
calib1.pose.position.z = -.23
calib1.pose.orientation.x = 1
calib1.pose.orientation.y = 0
calib1.pose.orientation.z = 0
calib1.pose.orientation.w = 0
plan = planner_left.plan_to_pose(calib1, obstacles)
raw_input("Press <Enter> to move the left arm to calib1 position: ")
if not planner_left.execute_plan(plan):
raise Exception("Execution failed")
else:
break
except Exception as e:
print e
while not rospy.is_shutdown():
try:
#intermidiate_to_fruit stage: move to the top of the fruit location and open the gripper
calib1 = PoseStamped()
calib1.header.frame_id = "base"
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
##
## Add the obstacle to the planning scene here
##
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 0.1;
# orien_const.weight = 1.0;
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
x, y, z = 0.47, -0.85, 0.07
else:
x, y, z = 0.8, 0.05, 0.07
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
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_1, [])
raw_input(
"Press <Enter> to move the right arm to goal pose 1: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
traceback.print_exc()
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_2, [])
raw_input(
"Press <Enter> to move the right arm to goal pose 2: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
plan = planner.plan_to_pose(goal_3, [])
raw_input(
"Press <Enter> to move the right arm to goal pose 3: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
global prev_msg
global sec_pre
plandraw = PathPlanner('right_arm')
# plandraw.grip?per_op
plandraw.start_position()
## BOX
box_size = np.array([2.4, 2.4, 0.1])
box_pose = PoseStamped()
box_pose.header.stamp = rospy.Time.now()
box_pose.header.frame_id = "base"
box_pose.pose.position.x = 0
box_pose.pose.position.y = 0
box_pose.pose.position.z = -0.4
box_pose.pose.orientation.x = 0.00
box_pose.pose.orientation.y = 0.00
box_pose.pose.orientation.z = 0.00
box_pose.pose.orientation.w = 1.00
plandraw.add_box_obstacle(box_size, "box1", box_pose)
box_size2 = np.array([2.4, 2.4, 0.1])
box_pose2 = PoseStamped()
box_pose2.header.stamp = rospy.Time.now()
box_pose2.header.frame_id = "base"
box_pose2.pose.position.x = 0
box_pose2.pose.position.y = 0
box_pose2.pose.position.z = 1
box_pose2.pose.orientation.x = 0.00
box_pose2.pose.orientation.y = 0.00
box_pose2.pose.orientation.z = 0.00
box_pose2.pose.orientation.w = 1.00
plandraw.add_box_obstacle(box_size2, "box2", box_pose2)
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper_tip"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
def set_use_pen(pen_id, goal_1):
if pen_id == 0:
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -0.9848078
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = -0.1736482
if pen_id == 1:
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
if pen_id == 2:
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -0.9848078
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.1736482
waypoints = []
while not rospy.is_shutdown():
#raw_input("~~~~~~~~~~~~!!!!!!!!!!!!")
while not rospy.is_shutdown():
try:
while len(queue):
print(len(queue))
cur = queue.popleft()
x, y, z = cur.position_x, cur.position_y, cur.position_z
x -= 0.085 # ada different coordinate
z += 0.03
if cur.status_type != "edge_grad":
# ti bi !!!!! luo bi !!!!
if cur.status_type == "starting":
print("start")
# waypoints = []
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 + 0.12
#Orientation as a quaternion
# [0.766, -0.623, 0.139, -0.082]
# [-0.077408, 0.99027, -0.024714, ]
set_use_pen(cur.pen_type, goal_1)
waypoints.append(copy.deepcopy(goal_1.pose))
goal_1.pose.position.z -= 0.12
waypoints.append(copy.deepcopy(goal_1.pose))
# plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints)
# if not plandraw.execute_plan(plan):
# raise Exception("Starting execution failed")
# else:
# queue.pop(0)
elif cur.status_type == "next_point":
print("next")
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.459962
# goal_1.pose.orientation.y = -0.7666033
# goal_1.pose.orientation.z = 0.0
# goal_1.pose.orientation.w = -0.4480562
set_use_pen(cur.pen_type, goal_1)
# waypoints = []
waypoints.append(copy.deepcopy(goal_1.pose))
# plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints)
# if not plandraw.execute_plan(plan):
# raise Exception("Execution failed, point is ", cur)
# else:
# queue.pop(0)
elif cur.status_type == "ending":
print("ppppppp ", cur)
# mmm = plandraw.get_cur_pos().pose
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 + 0.12
#Orientation as a quaternion
set_use_pen(1, goal_1)
# waypoints = []
waypoints.append(copy.deepcopy(goal_1.pose))
plan = plandraw.plan_to_pose(goal_1, [], waypoints)
waypoints = []
# queue.pop(0)
if not plandraw.execute_plan(plan):
raise Exception("Execution failed")
print("ti bi")
# rospy.sleep(5)
#raw_input("Press <Enter> to move next!!!")
except Exception as e:
print e
else:
#print("lllllllllllllllllllll")
break
def play_song(note_pos):
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
z_offset = 0.05
##
## Add the obstacle to the planning scene here
##
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 0.1;
# orien_const.weight = 1.0;
# # Note 1 for testing
# note1 = Vector3()
# note1.x = 0.6
# note1.y = -0.3
# note1.z = 0.1
# # Note 2 for testing
# note2 = Vector3()
# note2.x = 0.6
# note2.y = -0.25
# note2.z = 0.1
# while not rospy.is_shutdown():
# Iterate through all note positions for the song
for i, pos in enumerate(note_pos):
# Loop until that position is reached
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = pos.x
goal_1.pose.position.y = pos.y
goal_1.pose.position.z = pos.z
#Orientation as a quaternion, facing straight down
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
plan = planner.plan_to_pose(goal_1, list())
# raw_input("Press <Enter> to move the right arm to goal pose {}: ".format(i))
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def main():
"""
Main Script
"""
# Make sure that you've looked at and understand path_planner.py before starting
plannerR = PathPlanner("right_arm")
plannerL = PathPlanner("left_arm")
right_gripper = robot_gripper.Gripper('right')
left_gripper = robot_gripper.Gripper('left')
# Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
# Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students
# Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
# Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
# right_arm = Limb("right")
# controller = Controller(Kp, Kd, Ki, Kw, right_arm)
##
## Add the obstacle to the planning scene here
##
obstacle1 = PoseStamped()
obstacle1.header.frame_id = "base"
#x, y, and z position
obstacle1.pose.position.x = 0.4
obstacle1.pose.position.y = 0
obstacle1.pose.position.z = -0.29
#Orientation as a quaternion
obstacle1.pose.orientation.x = 0.0
obstacle1.pose.orientation.y = 0.0
obstacle1.pose.orientation.z = 0.0
obstacle1.pose.orientation.w = 1
plannerR.add_box_obstacle(np.array([1.2, 1.2, .005]), "tableBox",
obstacle1)
# obstacle2 = PoseStamped()
# obstacle2.header.frame_id = "wall"
# #x, y, and z position
# obstacle2.pose.position.x = 0.466
# obstacle2.pose.position.y = -0.670
# obstacle2.pose.position.z = -0.005
# #Orientation as a quaternion
# obstacle2.pose.orientation.x = 0.694
# obstacle2.pose.orientation.y = -0.669
# obstacle2.pose.orientation.z = 0.251
# obstacle2.pose.orientation.w = -0.092
# planner.add_box_obstacle(np.array([0.01, 2, 2]), "wall", obstacle2)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
# orien_const = OrientationConstraint()
# orien_const.link_name = "right_gripper";
# orien_const.header.frame_id = "base";
# orien_const.orientation.y = -1.0;
# orien_const.absolute_x_axis_tolerance = 0.1;
# orien_const.absolute_y_axis_tolerance = 0.1;
# orien_const.absolute_z_axis_tolerance = 0.1;
# orien_const.weight = 1.0;
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.440
goal_3.pose.position.y = -0.012
goal_3.pose.position.z = 0.549
#Orientation as a quaternion
goal_3.pose.orientation.x = -0.314
goal_3.pose.orientation.y = -0.389
goal_3.pose.orientation.z = 0.432
goal_3.pose.orientation.w = 0.749
#plan = plannerR.plan_to_pose(goal_3, list())
raw_input(
"Press <Enter> to move the right arm to goal pose 3: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_3, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.448
goal_2.pose.position.y = -0.047
goal_2.pose.position.z = -0.245
#Orientation as a quaternion
goal_2.pose.orientation.x = 0
goal_2.pose.orientation.y = 1
goal_2.pose.orientation.z = 0
goal_2.pose.orientation.w = 0
#plan = plannerR.plan_to_pose(goal_2, list())
raw_input(
"Press <Enter> to move the right arm to goal pose 2: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_2, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
while not rospy.is_shutdown():
try:
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.448
goal_1.pose.position.y = -0.047
goal_1.pose.position.z = -0.245
#Orientation as a quaternion
goal_1.pose.orientation.x = 1
goal_1.pose.orientation.y = 0
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 0
# Might have to edit this . . .
#plan = plannerR.plan_to_pose(goal_1, list()) # put orien_const here
raw_input(
"Press <Enter> to move the right arm to goal pose 1: ")
if not plannerR.execute_plan(
plannerR.plan_to_pose(goal_1, list())):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def move(x, y, z, box_in):
planner = PathPlanner("right_arm")
#TODO: Add constraint restricting z > 0.
#Wait for the IK service to become available
#rospy.wait_for_service('compute_ik')
#rospy.init_node('service_query')
place_holder = {'images': [], 'camera_infos': []}
#rospy.Subscriber("/cameras/head_camera/image", Image, lambda x: place_holder['images'].append(x))
#rospy.Subscriber("/cameras/head_camera/camera_info", CameraInfo, lambda x: place_holder['camera_infos'].append(x))
limb = Limb("right")
#print(limb)
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
count = 0
print(limb.endpoint_pose())
calibration_points = []
# while not rospy.is_shutdown() and count < 3:
# #calibration points 1-3, closest to robot right = origin, closest to robot left, id_card 3 3/8 inches forward from first point
# '''
# /cameras/head_camera/camera_info
# /cameras/head_camera/camera_info_std
# /cameras/head_camera/image
# '''
# count += 1
# raw_input("calibrate point " + str(count))
# pos = limb.endpoint_pose()
# calibration_points.append({})
# calibration_points[-1]['world_coordinates'] = (pos['position'].x, pos['position'].y, pos['position'].z)
# #calibration_points[-1]['camera_info'] = place_holder['camera_infos'][-1]
# #calibration_points[-1]['image'] = place_holder['images'][-1]
# for i in calibration_points:
# print(i['world_coordinates'])
if ROBOT == "sawyer":
Kp = 0.2 * np.array([0.4, 2, 1.7, 1.5, 2, 2, 3])
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
else:
Kp = 0.45 * np.array([0.8, 2.5, 1.7, 2.2, 2.4, 3, 4])
Kd = 0.015 * np.array([2, 1, 2, 0.5, 0.8, 0.8, 0.8])
Ki = 0.01 * np.array([1.4, 1.4, 1.4, 1, 0.6, 0.6, 0.6])
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9])
## Add the controller here! IF you uncomment below, you should get the checkoff!
c = Controller(Kp, Ki, Kd, Kw, Limb('right'))
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
box = PoseStamped()
box.header.frame_id = "base"
box.pose.position.x = box_in.pose.position.x
box.pose.position.y = box_in.pose.position.y
box.pose.position.z = box_in.pose.position.z
# box.pose.position.x = 0.5
# box.pose.position.y = 0.0
# box.pose.position.z = 0.0
box.pose.orientation.x = 0.0
box.pose.orientation.y = 0.0
box.pose.orientation.z = 0.0
box.pose.orientation.w = 1.0
planner.add_box_obstacle((3, 4, 0.10), "box", box)
# planner.add_box_obstacle((box_in.pose.position.x, box_in.pose.position.y, box_in.pose.position.z), "box", box)
# ree = input("some text")
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
_x, _y, _z = .67, -.2, .3
#x, y, z = .95, .11, .02
#x, y, z = .7, -.4, .2
print(0)
else:
raise Exception("not configured for sawyer yet.")
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
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_1, [orien_const])
raw_input("Press <Enter> to move the right arm to goal pose 1: ")
start = time.time()
if not c.execute_path(plan):
raise Exception("Execution failed")
runtime = time.time() - start
with open(
'/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt',
'a') as f:
f.write(str(runtime) + ' move above point \n')
except Exception as e:
print(e)
traceback.print_exc()
else:
break
while not rospy.is_shutdown():
try:
if ROBOT == "baxter":
#x, y, z = .67, -.2, 0
# x, y, z = .79, .04, -.04
pass
else:
raise Exception("not configured for sawyer yet.")
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = x
goal_2.pose.position.y = y
goal_2.pose.position.z = z
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
# Might have to edit this . . .
plan = planner.plan_to_pose(goal_2, [orien_const])
raw_input("Press <Enter> to move the right arm to goal pose 2: ")
start = time.time()
if not c.execute_path(plan):
raise Exception("Execution failed")
runtime = time.time() - start
with open(
'/home/cc/ee106a/fa19/class/ee106a-aby/ros_workspaces/final_project_kin/src/kin/src/scripts/timing.txt',
'a') as f:
f.write(str(runtime) + ' move to point \n')
except Exception as e:
print(e)
traceback.print_exc()
else:
break
