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)
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
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)
# is a jointspace or torque controller, it should return a trajectory where the positions
# and velocities are the positions and velocities of each joint.
robot_trajectory = get_trajectory(args.task, current_pos, tag_pos,
args.num_way, args.controller_name, limb,
kin, args.rate)
# This is a wrapper around MoveIt! for you to use. We use MoveIt! to go to the start position
# of the trajectory
planner = PathPlanner('{}_arm'.format(args.arm))
if args.controller_name == "workspace":
pose = create_pose_stamped_from_pos_quat(
robot_trajectory.joint_trajectory.points[0].positions,
[0, 1, 0, 0], 'base')
plan = planner.plan_to_pose(pose)
else:
plan = planner.plan_to_joint_pos(
robot_trajectory.joint_trajectory.points[0].positions)
planner.execute_plan(plan)
if args.moveit:
# LAB 1 PART A
# by publishing the trajectory to the move_group/display_planned_path topic, you should
# be able to view it in RViz. You will have to click the "loop animation" setting in
# the planned path section of MoveIt! in the menu on the left side of the screen.
pub = rospy.Publisher('move_group/display_planned_path',
DisplayTrajectory,
queue_size=10)
disp_traj = DisplayTrajectory()
disp_traj.trajectory.append(robot_trajectory)
# disp_traj.trajectory_start = planner._group.get_current_joint_values()
disp_traj.trajectory_start = RobotState()
pub.publish(disp_traj)
-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():
"""
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)
