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 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 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 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()
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
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)