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
"""
#############
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)
#Create a path constraint for the arm
#UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.z = -1.0
orien_const.absolute_x_axis_tolerance = 0.2
orien_const.absolute_y_axis_tolerance = 0.2
orien_const.absolute_z_axis_tolerance = 0.2
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 = 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
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())
## 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_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")
def talker(origin_frame):
#Run this program as a new node in the ROS computation graph
#called /talker.
rospy.init_node('main_controller', anonymous=True)
#Create an instance of the rospy.Publisher object which we can
#use to publish messages to a topic.
pub = rospy.Publisher('gripper', String, queue_size=10)
#Create a tf buffer, which is primed with a tf listener
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
planner = PathPlanner("right_arm")
right = Limb('right')
# rj = right.joint_names()
r = rospy.Rate(10)
# Loop until the node is killed with Ctrl-C
while not rospy.is_shutdown():
try:
pub_string = raw_input('Type [open] or [close]: ')
# Publish our string to the 'chatter_talk' topic
pub.publish(pub_string)
while not rospy.is_shutdown():
try:
raw_input("Press <Enter> to move the right arm to goal pose 1: ")
plan = planner.plan_to_joint_default()
# print(plan)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
time.sleep(2)
except Exception as e:
print e
traceback.print_exc()
else:
break
trans_to_wrist = tfBuffer.lookup_transform(origin_frame, "right_hand", rospy.Time())
# trans_to_tip = tfBuffer.lookup_transform(origin_frame, "right_gripper", rospy.Time())
trans_to_base = tfBuffer.lookup_transform(origin_frame, "right_lower_shoulder", rospy.Time())
trans_to_cup = tfBuffer.lookup_transform(origin_frame, "cup_center", rospy.Time())
gun = [0,0,0]
base = [0,0,0]
goal = [0,0,0]
# gun[0] = (trans_to_wrist.transform.translation.x+trans_to_tip.transform.translation.x)/2
# gun[1] = (trans_to_wrist.transform.translation.y+trans_to_tip.transform.translation.y)/2
# gun[2] = (trans_to_wrist.transform.translation.z+trans_to_tip.transform.translation.z)/2
gun[0] = trans_to_wrist.transform.translation.x
gun[1] = trans_to_wrist.transform.translation.y
gun[2] = trans_to_wrist.transform.translation.z
base[0] = trans_to_base.transform.translation.x
base[1] = trans_to_base.transform.translation.y
base[2] = trans_to_base.transform.translation.z
goal[0] = trans_to_cup.transform.translation.x
goal[1] = trans_to_cup.transform.translation.y
# height of cup
goal[2] = 0.12065/2
theta1 = Projectile.calc_xy(goal, base, gun)
print("theta1 = "+str(theta1))
while not rospy.is_shutdown():
try:
raw_input("Press <Enter> to move the right arm to theta1: ")
goal_joints = right.joint_angles()
goal_joints["right_s0"] += np.radians(theta1)
plan = planner.plan_to_joint(goal_joints)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
time.sleep(2)
except Exception as e:
print e
traceback.print_exc()
else:
break
trans_to_wrist = tfBuffer.lookup_transform(origin_frame, "right_hand", rospy.Time())
# trans_to_tip = tfBuffer.lookup_transform(origin_frame, "right_gripper", rospy.Time())
# gun[0] = (trans_to_wrist.transform.translation.x+trans_to_tip.transform.translation.x)/2
# gun[1] = (trans_to_wrist.transform.translation.y+trans_to_tip.transform.translation.y)/2
# gun[2] = (trans_to_wrist.transform.translation.z+trans_to_tip.transform.translation.z)/2
gun[0] = trans_to_wrist.transform.translation.x
gun[1] = trans_to_wrist.transform.translation.y
gun[2] = trans_to_wrist.transform.translation.z
theta2 = gripper_sub.aim_z(gun,goal)
print("theta2 = "+str(np.degrees(theta2)))
while not rospy.is_shutdown():
try:
raw_input("Press <Enter> to move the right arm to theta2: ")
goal_joints = right.joint_angles()
goal_joints["right_w2"] = pi/2-theta2 + np.radians(OFFSET)
plan = planner.plan_to_joint(goal_joints)
if not planner.execute_plan(plan):
raise Exception("Execution failed")
time.sleep(2)
except Exception as e:
print e
traceback.print_exc()
else:
break
# Construct a string that we want to publish
# (In Python, the "%" operator functions similarly
# to sprintf in C or MATLAB)
pub_string = raw_input('Type [open] or [close]: ')
# Publish our string to the 'chatter_talk' topic
pub.publish(pub_string)
# print(np.degrees(joint_angles))
# print(delta)
# if np.amax(error) >= .1:
# print("go")
# gripper_sub.default_state(JOINT_DEFAULT)
# aim_xy(end_coord, base_coord, cup_coord, joint_state)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
print('loading')
pass
r.sleep()
class BaxterLearning():
"""docstring for BaxterLearning"""
def __init__(self):
if not self.load_parameters(): sys.exit(1)
self._limb = baxter_interface.Limb(self._arm)
self._kin = baxter_kinematics(self._arm)
self._planner = PathPlanner('{}_arm'.format(self._arm))
rospy.on_shutdown(self.shutdown)
plan = self._planner.plan_to_joint_pos(
np.array([-0.6, -0.4, -0.5, 0.6, -0.4, 1.1, -0.5]))
self._planner.execute_plan(plan)
rospy.sleep(5)
################################## Tensorflow bullshit
if self._learning_bool:
# I DON'T KNOW HOW THESE WORK
#define placeholders
observation_space = spaces.Box(low=-100,
high=100,
shape=(21, ),
dtype=np.float32)
action_space = spaces.Box(low=-50,
high=50,
shape=(56, ),
dtype=np.float32)
self._x_ph, self._u_ph = core.placeholders_from_spaces(
observation_space, action_space)
#define actor critic
#TODO add in central way to accept arguments
self._pi, self._logp, self._logp_pi, self._v = core.mlp_actor_critic(
self._x_ph,
self._u_ph,
hidden_sizes=(128, 2),
action_space=action_space)
# POLY_ORDER = 2
# self._pi, self._logp, self._logp_pi, self._v = core.polynomial_actor_critic(
# self._x_ph, self._u_ph, POLY_ORDER, action_space=action_space)
#start up tensorflow graph
var_counts = tuple(core.count_vars(scope) for scope in [u'pi'])
print(u'\nYoyoyoyyoyo Number of parameters: \t pi: %d\n' %
var_counts)
self._tf_vars = [
v for v in tf.trainable_variables() if u'pi' in v.name
]
self._num_tf_vars = sum(
[np.prod(v.shape.as_list()) for v in self._tf_vars])
print("tf vars is of length: ", len(self._tf_vars))
print("total trainable vars: ", len(tf.trainable_variables()))
self._sess = tf.Session()
self._sess.run(tf.global_variables_initializer())
print("total trainable vars: ", len(tf.trainable_variables()))
self._last_time = rospy.Time.now().to_sec()
current_position = utils.get_joint_positions(self._limb).reshape(
(7, 1))
current_velocity = utils.get_joint_velocities(self._limb).reshape(
(7, 1))
self._last_x = np.vstack([current_position, current_velocity])
self._last_xbar = self._last_x
if self._learning_bool:
self._last_a = self._sess.run(self._pi,
feed_dict={
self._x_ph:
self.preprocess_state(
self._last_x).reshape(1, -1)
})
else:
self._last_a = np.zeros((1, 56))
#################################### Set Tensorflow from saved params
self._READ_PARAMS = self._is_test_set
PARAM_INDEX = 201 ## This is the index of the learned parameters that you'll use (which epoch)
if self._learning_bool:
if self._READ_PARAMS:
import dill
# Put ABSOLUTE path to location of learned_params.pkl here,
# then remove the NotImplementedError. (This will probably be the
# same as the PREFIX variable in data_collector.py)
# eg. PREFIX = "/home/cc/ee106a/fa19/staff/ee106a-taf/Desktop/data"
PREFIX = "/home/cc/ee106b/sp20/staff/ee106b-laa/Desktop/data/read_params"
# raise NotImplementedError
lp = dill.load(open(PREFIX + "/learned_params.pkl", "rb"))
print("Running training set using data from epoch number: " +
str(PARAM_INDEX))
param_list = []
for param in lp[PARAM_INDEX][1]:
p_msg = Parameters(param)
param_list.append(p_msg)
lp_msg = LearnedParameters(param_list)
self._sess.run(
tf.assign(
tf.get_default_graph().get_tensor_by_name(
'pi/log_std:0'), tf.zeros((56, ))))
self.params_callback(lp_msg)
self._last_a = self._sess.run(
self._pi,
feed_dict={
self._x_ph:
self.preprocess_state(self._last_x).reshape(1, -1)
})
##################################### Controller params
self._A = np.vstack([
np.hstack([np.zeros((7, 7)), np.eye(7)]),
np.hstack([np.zeros((7, 7)), np.zeros((7, 7))])
])
self._B = np.vstack([np.zeros((7, 7)), np.eye(7)])
Q = 0.2 * np.diag([
1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0,
1.0
])
R = np.eye(7)
def solve_lqr(A, B, Q, R):
P = solve_continuous_are(A, B, Q, R)
K = np.dot(np.dot(np.linalg.inv(R), B.T), P)
return K
self._K = solve_lqr(self._A, self._B, Q, R)
# self._Kp = 6*np.diag(np.array([1, 1, 1.5, 1.5, 1, 1, 1]))
# self._Kv = 5*np.diag(np.array([2, 2, 1, 1, 0.8, 0.3, 0.3]))
# self._Kp = 9*np.diag(np.array([4, 6, 4, 8, 1, 5, 1]))
# self._Kv = 5*np.diag(np.array([2, 3, 2, 4, 0.8, 1.5, 0.3]))
# WORKS WELL FOR TRAINING
self._Kp = 6 * np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
self._Kv = 6 * np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
# Tune down for testing except I didn't
# self._Kp = 5*np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
# self._Kv = 5*np.diag(np.array([1, 1, 1, 1, 1, 1, 1]))
if not self.register_callbacks(): sys.exit(1)
def load_parameters(self):
if not rospy.has_param("~baxter/arm"):
return False
self._arm = rospy.get_param("~baxter/arm")
if not rospy.has_param("~learning"):
return False
self._learning_bool = rospy.get_param("~learning")
if not rospy.has_param("~topics/ref"):
return False
self._ref_topic = rospy.get_param("~topics/ref")
if not rospy.has_param("~topics/params"):
return False
self._params_topic = rospy.get_param("~topics/params")
if not rospy.has_param("~topics/transitions"):
return False
self._transitions_topic = rospy.get_param("~topics/transitions")
if not rospy.has_param("~topics/linear_system_reset"):
return False
self._reset_topic = rospy.get_param("~topics/linear_system_reset")
if not rospy.has_param("~topics/integration_reset"):
return False
self._int_reset_topic = rospy.get_param("~topics/integration_reset")
if not rospy.has_param("~topics/data"):
return False
self._data_topic = rospy.get_param("~topics/data")
if not rospy.has_param("~test_set"):
return False
self._is_test_set = rospy.get_param("~test_set")
return True
def register_callbacks(self):
if not self._is_test_set:
self._params_sub = rospy.Subscriber(self._params_topic,
LearnedParameters,
self.params_callback)
self._ref_sub = rospy.Subscriber(self._ref_topic, Reference,
self.ref_callback)
self._reset_sub = rospy.Subscriber(self._reset_topic, Empty,
self.linear_system_reset_callback)
self._int_reset_sub = rospy.Subscriber(self._int_reset_topic, Empty,
self.integration_reset_callback)
self._transitions_pub = rospy.Publisher(self._transitions_topic,
Transition)
self._data_pub = rospy.Publisher(self._data_topic, DataLog)
return True
def params_callback(self, msg):
t = rospy.Time.now().to_sec()
sq_norm_old_params = 0.0
sq_norm_difference = 0.0
norm_params = []
num_params = 0
for p, v in zip(msg.params, self._tf_vars):
num_params += len(p.params)
print("P is len ", len(p.params), ", and v is len ",
np.prod(v.shape.as_list()))
assert (len(p.params) == np.prod(v.shape.as_list()))
reshaped_p = np.array(p.params).reshape(v.shape.as_list())
norm_params.append(np.linalg.norm(reshaped_p))
v_actual = self._sess.run(v)
sq_norm_old_params += np.linalg.norm(v_actual)**2
sq_norm_difference += np.linalg.norm(v_actual -
np.array(reshaped_p))**2
self._sess.run(tf.assign(v, reshaped_p))
rospy.loginfo("Updated tf params in controller.")
rospy.loginfo("Parameters changed by %f on average." %
(sq_norm_difference / float(num_params)))
rospy.loginfo("Parameter group norms: " + str(norm_params))
rospy.loginfo("Params callback took %f seconds." %
(rospy.Time.now().to_sec() - t))
def ref_callback(self, msg):
# Get/log state data
ref = msg
dt = rospy.Time.now().to_sec() - self._last_time
self._last_time = rospy.Time.now().to_sec()
current_position = utils.get_joint_positions(self._limb).reshape(
(7, 1))
current_velocity = utils.get_joint_velocities(self._limb).reshape(
(7, 1))
current_state = State(current_position, current_velocity)
# get dynamics info
positions_dict = utils.joint_array_to_dict(current_position,
self._limb)
velocity_dict = utils.joint_array_to_dict(current_velocity, self._limb)
inertia = self._kin.inertia(positions_dict)
# coriolis = self._kin.coriolis(positions_dict, velocity_dict)[0][0]
# coriolis = np.array([float(coriolis[i]) for i in range(7)]).reshape((7,1))
coriolis = self._kin.coriolis(positions_dict, velocity_dict).reshape(
(7, 1))
# gravity_wrench = np.array([0,0,0.981,0,0,0]).reshape((6,1))
# gravity_jointspace = (np.matmul(self._kin.jacobian_transpose(positions_dict), gravity_wrench))
# gravity = (np.matmul(inertia, gravity_jointspace)).reshape((7,1))
gravity = 0.075 * self._kin.gravity(positions_dict).reshape((7, 1))
# Linear Control
error = np.array(ref.setpoint.position).reshape(
(7, 1)) - current_position
d_error = np.array(ref.setpoint.velocity).reshape(
(7, 1)) - current_velocity
# d_error = np.zeros((7,1))
error_stack = np.vstack([error, d_error])
v = np.matmul(self._Kv, d_error).reshape(
(7, 1)) + np.matmul(self._Kp, error).reshape(
(7, 1)) + np.array(ref.feed_forward).reshape((7, 1))
# v = np.array(ref.feed_forward).reshape((7,1))
# v = np.matmul(self._K, error_stack).reshape((7,1))
# v = np.zeros((7,1))
# print current_position
##### Nonlinear control
x = np.vstack([current_position, current_velocity])
xbar = np.vstack([
np.array(ref.setpoint.position).reshape((7, 1)),
np.array(ref.setpoint.velocity).reshape((7, 1))
])
if self._learning_bool:
a = self._sess.run(self._pi,
feed_dict={
self._x_ph:
self.preprocess_state(x).reshape(1, -1)
})
else:
a = np.zeros((1, 56))
m2, f2 = np.split(0.1 * a[0], [49])
m2 = m2.reshape((7, 7))
f2 = f2.reshape((7, 1))
K = np.hstack([self._Kp, self._Kv])
x_predict = np.matmul(expm((self._A - np.matmul(self._B, K))*dt), self._last_x) + \
np.matmul(np.matmul(self._B, K), self._last_xbar)*dt
t_msg = Transition()
t_msg.x = list(self._last_x.flatten())
t_msg.a = list(self._last_a.flatten())
t_msg.r = -np.linalg.norm(x - x_predict, 2)
if self._learning_bool and not self._is_test_set:
self._transitions_pub.publish(t_msg)
data = DataLog(current_state, ref.setpoint, t_msg)
self._last_x = x
self._last_xbar = xbar
self._last_a = a
self._data_pub.publish(data)
torque_lim = np.array([4, 4, 4, 4, 2, 2, 2]).reshape((7, 1))
torque = np.matmul(inertia + m2, v) + coriolis + gravity + f2
torque = np.clip(torque, -torque_lim, torque_lim).reshape((7, 1))
torque_dict = utils.joint_array_to_dict(torque, self._limb)
self._limb.set_joint_torques(torque_dict)
def linear_system_reset_callback(self, msg):
# plan = self._planner.plan_to_joint_pos(np.zeros(7))
# self._planner.execute_plan(plan)
pass
def integration_reset_callback(self, msg):
self._last_time = rospy.Time.now().to_sec()
def preprocess_state(self, x0):
q = x0[0:7]
dq = x0[7:14]
x = np.hstack([np.sin(q), np.cos(q), dq])
return x
def shutdown(self):
"""
Code to run on shutdown. This is good practice for safety
"""
rospy.loginfo("Stopping Controller")
# Set velocities to zero
zero_vel_dict = utils.joint_array_to_dict(np.zeros(7), self._limb)
self._limb.set_joint_velocities(zero_vel_dict)
# self._planner.stop()
rospy.sleep(0.1)
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
##
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = -1.1
pose.pose.position.y = 0
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1, 1, 5], "left_side_wall", pose)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = 0
pose.pose.position.y = -1.0
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1, 1, 5], "right_side_wall", pose)
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = "base"
pose.pose.position.x = 0
pose.pose.position.y = 1.1
pose.pose.position.z = 0
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
planner.add_box_obstacle([1, 1, 5], "back_wall", pose)
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "ar_marker_1"
#orien_const.orientation.y = np.pi/2
#orien_const.orientation.w = 1;
orien_const.absolute_x_axis_tolerance = 1
orien_const.absolute_y_axis_tolerance = 1
orien_const.absolute_z_axis_tolerance = 1
orien_const.weight = .5
joint_const = JointConstraint()
# Constrain the position of a joint to be within a certain bound
joint_const.joint_name = "right_j6"
joint_const.position = 1.7314052734375
# the bound to be achieved is [position - tolerance_below, position + tolerance_above]
joint_const.tolerance_above = .2
joint_const.tolerance_below = .2
# A weighting factor for this constraint (denotes relative importance to other constraints. Closer to zero means less important)
joint_const.weight = .5
#for stage 2
# orien_const1 = OrientationConstraint()
# orien_const1.link_name = "right_gripper";
# orien_const1.header.frame_id = "ar_marker_1";
# #orien_const.orientation.y = np.pi/2
# #orien_const.orientation.w = 1;
# orien_const1.absolute_x_axis_tolerance = 0.5;
# orien_const1.absolute_y_axis_tolerance = 0.5;
# orien_const1.absolute_z_axis_tolerance = 0.5;
# orien_const1.weight = 1.0;
#rospy.sleep(1.0)
# while not rospy.is_shutdown():
# while not rospy.is_shutdown():
try:
#right_gripper_tip
goal_1 = PoseStamped()
goal_1.header.frame_id = "ar_marker_1"
#x, y, and z position
goal_1.pose.position.x = -0.28
goal_1.pose.position.y = -0.05
#removing the 6th layer
goal_1.pose.position.z = .38 #0.0825
# z was .1
#Orientation as a quaternion
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = 0
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 1
plan = planner.plan_to_pose_joint(goal_1, [joint_const])
if not planner.execute_plan(plan):
raise Exception("Execution failed0")
except Exception as e:
print e
#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"
#x,y,z position
calib1.pose.position.x = 0.35
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")
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
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)
# disp_traj.trajectory_start = planner._group.get_current_joint_values()
disp_traj.trajectory_start = RobotState()
pub.publish(disp_traj)
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)
def main():
"""
Main Script
"""
while not rospy.is_shutdown():
planner = PathPlanner("right_arm")
limb = Limb("right")
joint_angles = limb.joint_angles()
print(joint_angles)
camera_angle = {
'right_j6': 1.72249609375,
'right_j5': 0.31765625,
'right_j4': -0.069416015625,
'right_j3': 1.1111962890625,
'right_j2': 0.0664150390625,
'right_j1': -1.357775390625,
'right_j0': -0.0880478515625
}
limb.set_joint_positions(camera_angle)
limb.move_to_joint_positions(camera_angle,
timeout=15.0,
threshold=0.008726646,
test=None)
#drawing_angles = {'right_j6': -2.00561328125, 'right_j5': -1.9730205078125, 'right_j4': 1.5130146484375, 'right_j3': -1.0272568359375, 'right_j2': 1.24968359375, 'right_j1': -0.239498046875, 'right_j0': 0.4667275390625}
#print(joint_angles)
#drawing_angles = {'right_j6': -1.0133310546875, 'right_j5': -1.5432158203125, 'right_j4': 1.4599892578125, 'right_j3': -0.04361328125, 'right_j2': 1.6773486328125, 'right_j1': 0.019876953125, 'right_j0': 0.4214736328125}
drawing_angles = {
'right_j6': 1.9668515625,
'right_j5': 1.07505859375,
'right_j4': -0.2511611328125,
'right_j3': 0.782650390625,
'right_j2': 0.25496875,
'right_j1': -0.3268349609375,
'right_j0': 0.201146484375
}
raw_input("Press <Enter> to take picture: ")
waypoints_abstract = vision()
print(waypoints_abstract)
#ar coordinate :
x = 0.461067
y = -0.235305
#first get the solution of the maze
#solutionpoints = [(0,0),(-0.66,0.16), (-0.7, 0.4)]
# Make sure that you've looked at and understand path_planner.py before starting
tfBuffer = tf2_ros.Buffer()
tfListener = tf2_ros.TransformListener(tfBuffer)
r = rospy.Rate(10)
#find trans from
while not rospy.is_shutdown():
try:
trans = tfBuffer.lookup_transform('base', 'ar_marker_0',
rospy.Time()).transform
break
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
if tf2_ros.LookupException:
print("lookup")
elif tf2_ros.ConnectivityException:
print("connect")
elif tf2_ros.ExtrapolationException:
print("extra")
# print("not found")
pass
r.sleep()
mat = as_transformation_matrix(trans)
point_spaces = []
for point in waypoints_abstract:
# for point in solutionpoints:
point = np.array([point[0], point[1], 0, 1])
point_space = np.dot(mat, point)
point_spaces.append(point_space)
print(point_spaces)
length_of_points = len(point_spaces)
raw_input("Press <Enter> to move the right arm to drawing position: ")
limb.set_joint_positions(drawing_angles)
limb.move_to_joint_positions(drawing_angles,
timeout=15.0,
threshold=0.008726646,
test=None)
##
## Add the obstacle to the planning scene here
##
#add obstacle
size = [0.78, 1.0, 0.05]
name = "box"
pose = PoseStamped()
pose.header.frame_id = "base"
pose.pose.position.x = 0.77
pose.pose.position.y = 0.0
pose.pose.position.z = -0.18
#Orientation as a quaternion
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0
pose.pose.orientation.w = 1.0
planner.add_box_obstacle(size, name, pose)
#limb.set_joint_positions( drawing_angles)
#limb.move_to_joint_positions( drawing_angles, timeout=15.0, threshold=0.008726646, test=None)
#starting position
x, y, z = 0.67, 0.31, -0.107343
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = x
goal_1.pose.position.y = y
goal_1.pose.position.z = z
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
while not rospy.is_shutdown():
try:
waypoint = []
for point in point_spaces:
goal_1.pose.position.x = point[0]
goal_1.pose.position.y = point[1]
goal_1.pose.position.z = -0.113343 #set this value when put different marker
waypoint.append(copy.deepcopy(goal_1.pose))
plan, fraction = planner.plan_straight(waypoint, [])
print(fraction)
raw_input("Press <Enter> to move the right arm to draw: ")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
traceback.print_exc()
else:
break
raw_input("Press <Enter> to start again: ")
# 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.
message = rospy.wait_for_message("/colors_and_position",
ColorAndPositionPairs)
cubes = message.pairs
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
pose = PoseStamped()
pose.header.frame_id = "base"
pose.pose.position.x = 0.3
pose.pose.position.y = -0.3
pose.pose.position.z = -0.3
pose.pose.orientation.x = 0.00
pose.pose.orientation.y = 0.00
pose.pose.orientation.z = 0.00
pose.pose.orientation.w = 1.00
#planner.add_box_obstacle(np.array([2.0,0.01,0.3]), "obs1", pose)
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.5
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, [orien_const])
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
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.y = -0.3
goal_2.pose.position.z = 0.0
#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])
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, [orien_const])
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
-0.7995570902191504, -1.0230128644807106, 0, 1.9412961426428978, 0,
0.6088981529125705, 0
]
qr = [
0.7995570902191504, -1.0230128644807106, 0, 1.9412961426428978, 0,
0.6088981529125705, 0
]
elif sys.argv[1] == 'high_stretch':
ql = [
-0.7995570902191504, -0.3583801252107035, 0, 0.8317392022589729, 0,
1.072678159715874, 0
]
qr = [
0.7995570902191504, -0.3583801252107035, 0, 0.8317392022589729, 0,
1.072678159715874, 0
]
else:
print(
"Please run this script with either 'low_stretch' or 'high_stretch' specified as a command line argument."
)
sys.exit()
planner = PathPlanner('left_arm')
success, plan, time_taken, error_code = planner.plan_to_joint_pos(ql)
planner.execute_plan(plan)
planner = PathPlanner('right_arm')
success, plan, time_taken, error_code = planner.plan_to_joint_pos(qr)
planner.execute_plan(plan)
def main():
"""
Main Script
"""
# Path Planner - right_arm for sawyer
planner = PathPlanner("right_arm")
##
## OBSTACLES
##
# poses = PoseStamped()
# poses.pose.position.x = .5
# poses.pose.position.y = 0
# poses.pose.position.z = 0
# poses.pose.orientation.x = 0
# poses.pose.orientation.y = 0
# poses.pose.orientation.z = 0
# poses.pose.orientation.w = 1
# poses.header.frame_id = "base"
# planner.add_box_obstacle(np.array([.4,1.2,.1]), "Howard", poses)
# ORIENTATION CONSTRAINT
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
#FULL PATH
path = [[.8, .05, -.23, "1"], [.6, -.3, 0.0, "2"], [.6, -.1, .1, "3"]]
while not rospy.is_shutdown():
for pos in path:
while not rospy.is_shutdown():
try:
goal = PoseStamped()
goal.header.frame_id = "base"
goal.pose.position.x = pos[0]
goal.pose.position.y = pos[1]
goal.pose.position.z = pos[2]
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 = planner.plan_to_pose(goal, [orien_const])
raw_input(
"Press <Enter> to move the right arm to goal pose " +
pos[3] + ":")
if not planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
print e
traceback.print_exc()
else:
break
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 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
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():
"""
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 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():
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 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():
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 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():
"""
python scripts/main.py --help <------This prints out all the help messages
and describes what each parameter is.
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, arbitrary. Default: line')
parser.add_argument(
'-goal_point',
'-g',
type=float,
default=[0, 0, 0],
nargs=3,
help=
'Specify a goal point for line and circle trajectories. Ex: -g 0 0 0')
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="""
(Hz) This specifies how many loop iterations should occur per second.
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()
if args.moveit:
# We can't give moveit a workspace trajectory, so we make it
# impossible.
args.controller_name = 'jointspace'
rospy.init_node('moveit_node')
# this is used for sending commands (velocity, torque, etc) to the robot
limb = baxter_interface.Limb(args.arm)
# Choose an inverse kinematics solver. If you leave ik_solver as None, then the default
# KDL solver will be used. Otherwise, you can uncomment the next line which sets ik_solver.
# Then, the TRAC-IK inverse kinematics solver will be used. If you have trouble with inconsistency
# or poor solutions with one method, feel free to try the other one.
ik_solver = None
# ik_solver = IK("base", args.arm + "_gripper")
# A KDL instance for Baxter. This can be used for forward and inverse kinematics,
# computing jacobians etc.
kin = baxter_kinematics(args.arm)
# This is a wrapper around MoveIt! for you to use.
planner = PathPlanner('{}_arm'.format(args.arm))
# Get an appropriate RobotTrajectory for the task (circular, linear, or arbitrary MoveIt)
# If the controller is a workspace controller, this should return a trajectory where the
# positions and velocities are workspace configurations 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(limb, kin, ik_solver, planner, args)
if args.controller_name == "workspace":
config = robot_trajectory.joint_trajectory.points[0].positions
pose = create_pose_stamped_from_pos_quat(
[config[0], config[1], config[2]], # XYZ location
[config[3], config[4], config[5], config[6]
], # XYZW quaterion orientation
'base')
success, plan, time_taken, error_code = planner.plan_to_pose(pose)
planner.execute_plan(plan)
else:
start = robot_trajectory.joint_trajectory.points[0].positions
while not rospy.is_shutdown():
try:
# ONLY FOR EMERGENCIES!!! DON'T UNCOMMENT THE NEXT LINE UNLESS YOU KNOW WHAT YOU'RE DOING!!!
# limb.move_to_joint_positions(joint_array_to_dict(start, limb), timeout=7.0, threshold=0.0001)
success, plan, time_taken, error_code = planner.plan_to_joint_pos(
start)
planner.execute_plan(plan)
break
except moveit_commander.exception.MoveItCommanderException as e:
print(e)
print("Failed planning, retrying...")
if args.moveit:
# PROJECT 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 = RobotState()
pub.publish(disp_traj)
try:
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
if args.log:
node = roslaunch.core.Node("proj1_pkg",
"moveit_plot.py",
args="{} {}".format(
args.arm, args.rate))
launch = roslaunch.scriptapi.ROSLaunch()
launch.start()
process = launch.launch(node)
rospy.wait_for_service('start_logging')
start_service = rospy.ServiceProxy('start_logging', TriggerLogging)
request = TriggerLoggingRequest(path=robot_trajectory)
start_service(robot_trajectory)
planner.execute_plan(robot_trajectory)
if args.log:
r = rospy.Rate(1)
while process.is_alive():
r.sleep()
else:
# PROJECT 1 PART B
controller = get_controller(args.controller_name, limb, kin)
try:
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 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
"""
# Setting up head camera
head_camera = CameraController("left_hand_camera")
head_camera.resolution = (1280, 800)
head_camera.open()
print("setting balance")
happy = False
while not happy:
e = head_camera.exposure
print("exposue value: " + str(e))
e_val = int(input("new exposure value"))
head_camera.exposure = e_val
satisfaction = str(raw_input("satified? y/n"))
happy = 'y' == satisfaction
planner = PathPlanner("right_arm")
listener = tf.TransformListener()
grip = Gripper('right')
grip.calibrate()
rospy.sleep(3)
grip.open()
# creating the table obstacle so that Baxter doesn't hit it
table_size = np.array([.5, 1, 10])
table_pose = PoseStamped()
table_pose.header.frame_id = "base"
thickness = 1
table_pose.pose.position.x = .9
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = -.112 - thickness / 2
table_size = np.array([.5, 1, thickness])
planner.add_box_obstacle(table_size, "table", table_pose)
raw_input("gripper close")
grip.close()
# ###load cup positions found using cup_detection.py ran in virtual environment
# start_pos_xy = np.load(POURING_CUP_PATH)
# goal_pos_xy = np.load(RECEIVING_CUP_PATH)
# start_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET)
# goal_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET)
# #### END LOADING CUP POSITIONS
camera_subscriber = rospy.Subscriber("cameras/left_hand_camera/image", Image, get_img)
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
cam_pos= [0.188, -0.012, 0.750]
##
## 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;
horizontal = getQuaternion(np.array([0,1,0]), np.pi)
z_rot_pos = getQuaternion(np.array([0,0,1]), np.pi / 2)
orig = quatMult(z_rot_pos, horizontal)
orig = getQuaternion(np.array([0,1,0]), np.pi / 2)
#IN THE VIEW OF THE CAMERA
#CORNER1--------->CORNER2
# | |
# | |
# | |
#CORNER3 ------------|
width = 0.3
length = 0.6
CORNER1 = np.array([0.799, -0.524, -0.03])
CORNER2 = CORNER1 + np.array([-width, 0, 0])
CORNER3 = CORNER1 + np.array([0, length, 0])
#CREATE THE GRID
dir1 = CORNER2 - CORNER1
dir2 = CORNER3 - CORNER1
grid_vals = []
ret_vals = []
for i in range(0, 3):
for j in range(0, 4):
grid = CORNER1 + i * dir1 / 2 + j * dir2 / 3
grid_vals.append(grid)
ret_vals.append(np.array([grid[0], grid[1], OBJECT_HEIGHT]))
i = -1
while not rospy.is_shutdown():
for g in grid_vals:
i += 1
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]
y = - g[1] + cam_pos[1]
x = g[0] - cam_pos[0]
q = orig
#Orientation as a quaternion
goal_1.pose.orientation.x = q[1][0]
goal_1.pose.orientation.y = q[1][1]
goal_1.pose.orientation.z = q[1][2]
goal_1.pose.orientation.w = q[0]
plan = planner.plan_to_pose(goal_1, [])
if planner.execute_plan(plan):
# raise Exception("Execution failed")
rospy.sleep(1)
#grip.open()
raw_input("open")
rospy.sleep(1)
# plt.imshow(camera_image)
print("yay: " + str(i))
pos = listener.lookupTransform("/reference/right_gripper", "/reference/base", rospy.Time(0))[0]
print("move succesfully to " + str(pos))
fname = os.path.join(IMAGE_DIR, "calib_{}.jpg".format(i))
skimage.io.imsave(fname, camera_image)
print(e)
print("index: ", i)
else:
break
print(np.array(ret_vals))
# save the positions of the gripper so that the homography matrix can be calculated
np.save(POINTS_DIR, np.array(ret_vals))
print(np.load(POINTS_DIR))
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
class ReferenceGenerator(object):
"""docstring for ReferenceGenerator"""
def __init__(self):
if not self.load_parameters(): sys.exit(1)
self._planner = PathPlanner('{}_arm'.format(self._arm))
rospy.on_shutdown(self.shutdown)
if not self.register_callbacks(): sys.exit(1)
def load_parameters(self):
if not rospy.has_param("~baxter/arm"):
return False
self._arm = rospy.get_param("~baxter/arm")
if not rospy.has_param("~topics/ref"):
return False
self._ref_topic = rospy.get_param("~topics/ref")
if not rospy.has_param("~topics/linear_system_reset"):
return False
self._reset_topic = rospy.get_param("~topics/linear_system_reset")
if not rospy.has_param("~topics/integration_reset"):
return False
self._int_reset_topic = rospy.get_param("~topics/integration_reset")
if not rospy.has_param("~test_set"):
return False
self._is_test_set = rospy.get_param("~test_set")
return True
def register_callbacks(self):
self._reset_sub = rospy.Subscriber(self._reset_topic, Empty,
self.linear_system_reset_callback)
self._ref_pub = rospy.Publisher(self._ref_topic, Reference)
self._int_reset_pub = rospy.Publisher(self._int_reset_topic, Empty)
return True
def send_zeros(self):
setpoint = State(np.zeros(7), np.zeros(7))
feed_forward = np.zeros(7)
msg = Reference(setpoint, feed_forward)
while not rospy.is_shutdown():
self._ref_pub.publish(msg)
rospy.sleep(0.05)
def random_span(self, rate=20, number=50):
min_range = np.array([-0.9, -0.6, -0.9, 0.2, -1.0, 0.1, -0.8])
max_range = np.array([0.9, 0.6, 0.9, 1.0, 1.0, 1.3, 0.8])
r = rospy.Rate(rate)
while not rospy.is_shutdown():
position = (max_range - min_range) * np.random.random_sample(
min_range.shape) + min_range
setpoint = State(position, np.zeros(7))
feed_forward = np.zeros(7)
msg = Reference(setpoint, feed_forward)
count = 0
while not count > number and not rospy.is_shutdown():
self._ref_pub.publish(msg)
count = count + 1
r.sleep()
def dual_setpoints(self, rate=20, number=50):
min_range = np.array([-0.9, -0.6, -0.9, 0.2, -1.0, 0.1, -0.8])
max_range = np.array([0.9, 0.6, 0.9, 1.0, 1.0, 1.3, 0.8])
r = rospy.Rate(rate)
flag = True
while not rospy.is_shutdown():
if flag:
position = (max_range - min_range) * 0.2 + min_range
else:
position = (max_range - min_range) * 0.8 + min_range
setpoint = State(position, np.zeros(7))
feed_forward = np.zeros(7)
msg = Reference(setpoint, feed_forward)
count = 0
while not count > number and not rospy.is_shutdown():
self._ref_pub.publish(msg)
count = count + 1
r.sleep()
flag = not flag
# Doesn't work
def sinusoids(self, rate=20, period=2):
r = rospy.Rate(rate)
min_range = np.array([-0.9, -0.6, -0.9, 0.2, -1.0, 0.1, -0.8])
max_range = np.array([0.9, 0.6, 0.9, 1.0, 1.0, 1.3, 0.8])
middle = (min_range + max_range) / 2.0
amp = np.array([0.6, 0.4, 0.6, 0.3, 0.7, 0.3, 0.6])
count = 0
max_count = period * rate
while not rospy.is_shutdown():
position = middle + amp * np.sin(count / max_count * 2 * np.pi)
setpoint = State(position, np.zeros(7))
feed_forward = np.zeros(7)
msg = Reference(setpoint, feed_forward)
self._ref_pub.publish(msg)
count = count + 1
if count >= max_count:
count = 0
r.sleep()
# Doesn't work
def test_path(self):
position1 = np.array([-0.7, -0.5, -0.7, 0.4, -0.9, 0.3, -0.6])
position2 = np.array([0.7, 0.5, 0.7, 0.8, 0.9, 1.1, 0.6])
print "moving to A via moveit"
self._planner.set_max_velocity_scaling_factor(1.0)
while not rospy.is_shutdown():
try:
plan = self._planner.plan_to_joint_pos(position1)
# raw_input("Press enter to execute plan via moveit")
if not self._planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
pass
else:
break
rospy.sleep(1)
print "moving to B via control"
self._planner.set_max_velocity_scaling_factor(0.5)
plan = self._planner.plan_to_joint_pos(position2)
# raw_input("Press enter to execute plan via control")
self._int_reset_pub.publish(Empty())
self.execute_path(plan)
rospy.sleep(1)
rospy.spin()
# Doesn't work
def alternate(self):
position1 = np.array([-0.3, -0.2, -0.3, 0.7, -0.6, 0.7, -0.3])
position2 = np.array([-0.6, -0.4, -0.5, 0.6, -0.4, 1.1, -0.5])
while not rospy.is_shutdown():
print "moving to A via moveit"
self._planner.set_max_velocity_scaling_factor(1.0)
while not rospy.is_shutdown():
try:
plan = self._planner.plan_to_joint_pos(position1)
# raw_input("Press enter to execute plan via moveit")
if not self._planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
pass
else:
break
rospy.sleep(1)
# self._planner.stop()
rospy.sleep(0.5)
print "moving to B via control"
self._planner.set_max_velocity_scaling_factor(0.5)
plan = self._planner.plan_to_joint_pos(position2)
# raw_input("Press enter to execute plan via control")
self._int_reset_pub.publish(Empty())
self.execute_path(plan)
rospy.sleep(1)
# self._planner.stop()
rospy.sleep(0.5)
print "moving to B via moveit"
self._planner.set_max_velocity_scaling_factor(1.0)
while not rospy.is_shutdown():
try:
plan = self._planner.plan_to_joint_pos(position2)
# raw_input("Press enter to execute plan via moveit")
if not self._planner.execute_plan(plan):
raise Exception("Execution failed")
except Exception as e:
pass
else:
break
rospy.sleep(1)
# self._planner.stop()
rospy.sleep(0.5)
print "moving to A via control"
self._planner.set_max_velocity_scaling_factor(0.5)
plan = self._planner.plan_to_joint_pos(position1)
# raw_input("Press enter to execute plan via control")
self._int_reset_pub.publish(Empty())
self.execute_path(plan)
rospy.sleep(1)
# self._planner.stop()
rospy.sleep(0.5)
def linear_system_reset_callback(self, msg):
rospy.sleep(0.5)
def interpolate_path(self, path, t, current_index=0):
"""
interpolates over a :obj:`moveit_msgs.msg.RobotTrajectory` to produce desired
positions, velocities, and accelerations at a specified time
Parameters
----------
path : :obj:`moveit_msgs.msg.RobotTrajectory`
t : float
the time from start
current_index : int
waypoint index from which to start search
Returns
-------
target_position : 7x' or 6x' :obj:`numpy.ndarray`
desired positions
target_velocity : 7x' or 6x' :obj:`numpy.ndarray`
desired velocities
target_acceleration : 7x' or 6x' :obj:`numpy.ndarray`
desired accelerations
current_index : int
waypoint index at which search was terminated
"""
# a very small number (should be much smaller than rate)
epsilon = 0.0001
max_index = len(path.joint_trajectory.points) - 1
# If the time at current index is greater than the current time,
# start looking from the beginning
if (path.joint_trajectory.points[current_index].time_from_start.to_sec(
) > t):
current_index = 0
# Iterate forwards so that you're using the latest time
while (
not rospy.is_shutdown() and \
current_index < max_index and \
path.joint_trajectory.points[current_index+1].time_from_start.to_sec() < t+epsilon
):
current_index = current_index + 1
# Perform the interpolation
if current_index < max_index:
time_low = path.joint_trajectory.points[
current_index].time_from_start.to_sec()
time_high = path.joint_trajectory.points[
current_index + 1].time_from_start.to_sec()
target_position_low = np.array(
path.joint_trajectory.points[current_index].positions)
target_velocity_low = np.array(
path.joint_trajectory.points[current_index].velocities)
target_acceleration_low = np.array(
path.joint_trajectory.points[current_index].accelerations)
target_position_high = np.array(
path.joint_trajectory.points[current_index + 1].positions)
target_velocity_high = np.array(
path.joint_trajectory.points[current_index + 1].velocities)
target_acceleration_high = np.array(
path.joint_trajectory.points[current_index + 1].accelerations)
target_position = target_position_low + \
(t - time_low)/(time_high - time_low)*(target_position_high - target_position_low)
target_velocity = target_velocity_low + \
(t - time_low)/(time_high - time_low)*(target_velocity_high - target_velocity_low)
target_acceleration = target_acceleration_low + \
(t - time_low)/(time_high - time_low)*(target_acceleration_high - target_acceleration_low)
# If you're at the last waypoint, no interpolation is needed
else:
target_position = np.array(
path.joint_trajectory.points[current_index].positions)
target_velocity = np.array(
path.joint_trajectory.points[current_index].velocities)
target_acceleration = np.array(
path.joint_trajectory.points[current_index].velocities)
return (target_position, target_velocity, target_acceleration,
current_index)
def execute_path(self, path, rate=20):
"""
takes in a path and moves the baxter in order to follow the path.
Parameters
----------
path : :obj:`moveit_msgs.msg.RobotTrajectory`
rate : int
This specifies the control frequency in hz. 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
Returns
-------
bool
whether the controller completes the path or not
"""
# For interpolation
max_index = len(path.joint_trajectory.points) - 1
current_index = 0
# For timing
start_t = rospy.Time.now()
r = rospy.Rate(rate)
while not rospy.is_shutdown():
# Find the time from start
t = (rospy.Time.now() - start_t).to_sec()
# Get the desired position, velocity, and acceleration
(target_position, target_velocity, target_acceleration,
current_index) = self.interpolate_path(path, t, current_index)
# Run controller
setpoint = State(target_position, target_velocity)
feed_forward = target_acceleration
msg = Reference(setpoint, feed_forward)
self._ref_pub.publish(msg)
# Sleep for a bit (to let robot move)
r.sleep()
if current_index >= max_index:
break
return True
def shutdown(self):
pass
