def _wobble(self):
""" Velocity controlled sine wave motion of both arms around their
current configuration. The sine wave is randomly parametrized.
Adapted from
https://github.com/RethinkRobotics/
baxter_examples/blob/master/scripts/joint_velocity_wobbler.py
:return: True on completion
"""
rate = rospy.Rate(settings.recording_rate)
jns_robot = settings.joint_names(self._arm_robot)
jns_human = settings.joint_names(self._arm_human)
def make_v_func():
""" Create a randomly parametrized cosine function.
:return: A randomly parametrized cosine function to control a
specific joint.
"""
period_factor = rnd.uniform(0.3, 0.5)
amplitude_factor = rnd.uniform(0.1, 0.3)
def v_func(elapsed):
w = period_factor * elapsed
return amplitude_factor * math.cos(w * 2 * math.pi)
return v_func
v_funcs = [make_v_func() for _ in jns_robot]
def make_cmd(joint_names, elapsed):
""" Create joint velocity command.
:param joint_names: list of joint names.
:param elapsed: elapsed time in [s].
:return: Dictionary of joint name keys to joint velocity commands
"""
return {
jn: v_funcs[i](elapsed)
for i, jn in enumerate(joint_names)
}
elapsed = 0.0
start = rospy.get_time()
while not rospy.is_shutdown() and elapsed < settings.run_time:
self._pub_rate.publish(settings.recording_rate)
elapsed = rospy.get_time() - start
cmd = make_cmd(jns_robot, elapsed)
self._limb_robot.set_joint_velocities(cmd)
cmd = make_cmd(jns_human, elapsed)
self._limb_human.set_joint_velocities(cmd)
rate.sleep()
return True
def sample_from_workspace(hull, kin, lim, arm):
""" Sample Cartesian pose and corresponding seven-dimensional
configuration within the workspace.
:param hull: Delauny composition of the workspace.
:param kin: A pykdl kinematic instance.
:param lim: Dictionary of joint name keys to joint angle limit tuples.
:param arm: <left/right> arm.
:return: A tuple (pose, config).
:raise: ValueError if pose does not lie within workspace.
"""
# sample seven uniform random values
config = np.random.random_sample((7, ))
# transform to joint range
config = [
config[i] * (lim[i][1] - lim[i][0]) + lim[i][0]
for i in range(len(config))
]
cfg = {a: b for a, b in zip(joint_names(arm), config)}
# transform to Cartesian space using forward kinematics
pose = kin.forward_position_kinematics(joint_values=cfg)
# check if pose is in convex hull of workspace corners
if hull.find_simplex(pose[:3]) >= 0:
return pose, config
else:
raise ValueError("Sampled pose does not lie in workspace!")
def _inverse_kinematics(self, pose, arm):
""" Inverse kinematics of one Baxter arm.
Compute valid set of joint angles for a given Cartesian pose using the
inverse kinematics service.
:param pose: Desired Cartesian pose [x, y. z, a, b, c].
:return:
"""
if not isinstance(pose, list) and not (len(pose) == 6
or len(pose) == 7):
raise ValueError("Pose must be a list with 6 or 7 entries!")
qp = self._list_to_pose_stamped(pose, "base")
node = "ExternalTools/" + arm + "/PositionKinematicsNode/IKService"
ik_service = rospy.ServiceProxy(node, SolvePositionIK)
ik_request = SolvePositionIKRequest()
ik_request.pose_stamp.append(qp)
try:
rospy.wait_for_service(node, 5.0)
ik_response = ik_service(ik_request)
except (rospy.ServiceException, rospy.ROSException) as error_message:
raise Exception("Service request failed: %r" % (error_message, ))
if ik_response.isValid[0]:
# convert response to joint position control dictionary
cfg = dict(
zip(ik_response.joints[0].name,
ik_response.joints[0].position))
return np.asarray([cfg[jn] for jn in joint_names(arm)])
else:
raise ValueError("No valid joint configuration found")
def sample(self):
""" Sample configurations within the workspace and store them. """
arm = raw_input(" Sample configurations for 'left' or 'right' arm: ")
if arm not in ['left', 'right']:
raise ValueError("Must be 'left' or 'right' arm!")
kin = baxter_kinematics(arm)
hull = Delaunay(self._data[:, :3])
n_configs = 300
configs = np.empty((n_configs, 7))
poses = np.empty((n_configs, 7))
idx = 0
lim = [q_lim(arm)[jn] for jn in joint_names(arm)]
while idx < n_configs:
try:
poses[idx, :], configs[idx, :] = \
sample_from_workspace(hull, kin, lim, arm)
idx += 1
except ValueError:
pass
print "\n"
path = raw_input(" Where to save poses2.txt and configurations2.txt: ")
if not os.path.exists(path):
os.makedirs(path)
np.savetxt(os.path.join(path, 'configurations2.txt'),
configs,
delimiter=',',
header='s0, s1, e0, e1, w0, w1, w2')
np.savetxt(os.path.join(path, 'poses2.txt'),
poses,
delimiter=',',
header='px, py, pz, ox, oy, oz, ow')
def _one_sample(self):
""" One sample with automatically induced anomalies.
Baxter moves one limb (randomly or in a fixed order, depending on
self._experiment) between 10 pre-defined configurations. For each
movement it is randomly selected whether an anomaly is due. If yes,
the P, I and D parameters of a randomly selected joint (excluding the
second wrist joint w2) are modified for 0.5 seconds and returned to
their default values afterward.
:return: True on completion.
"""
idxs = self._select_configurations()
jns = settings.joint_names(self._arm)
zeros = set_dict(self._arm, *(0.0, ) * 7)
kpid = settings.kpid(self._arm)
tau_lim = settings.tau_lim(self._arm, scale=0.2)
for idx in idxs:
q_des = {a: b for a, b in zip(jns, self._configs[idx])}
if self._joints:
command = [
q_des[jn] for jn in self._rec_joint.get_header_cfg()[1:]
]
self._pub_cfg_des.publish(command=command)
anomaly_pars = None
if self._anomalies:
if self._sampler.shall_anomaly():
pm = self._sampler.sample_p_multiplier()
im = self._sampler.sample_i_multiplier()
dm = self._sampler.sample_d_multiplier()
add = 0.0
a_mode = 1 # 1-'Control', 2-'Feedback'
a_type = 0 # 0-'Modified', 1-'Removed'
# random sample joint with motion > 10.0 deg, if none is
# found, no anomaly will be induced!
q_curr = self._limb.joint_angles()
for i in range(len(jns)):
if rospy.is_shutdown():
break
joint_id = np.random.randint(0, len(jns))
jn = jns[joint_id]
if abs(q_curr[jn] - q_des[jn]) > np.deg2rad(10.0):
anomaly_pars = [
pm, im, dm, add, joint_id, a_mode, a_type
]
break
self._move_to_joint_positions(des_idx=idx,
dq_des=zeros,
kpid=kpid,
tau_lim=tau_lim,
anomaly_pars=anomaly_pars,
timeout=self._timeout)
self._limb.move_to_neutral()
return True
def compute_durations(self):
""" Compute minimum duration for motions between all combinations of
configurations in the list of configurations and store them in a file.
:return: A numpy array containing the mapping.
"""
import numpy as np
from baxter_data_acquisition.misc import set_dict
from baxter_data_acquisition.settings import (
joint_names,
dq_scale
)
from ipl_bb import BangBangInterpolator
path = raw_input(" List-of-configurations-file to load: ")
cfgs = self.load_data(path)
arm = raw_input(" Compute durations for 'left' or 'right' arm: ")
if arm not in ['left', 'right']:
raise ValueError("Must be 'left' or 'right' arm!")
# Convert list of configs into a list of baxter joint position
# command dictionaries
c = [{a: b for a, b in zip(joint_names(arm), cfgs[n, :])}
for n in range(cfgs.shape[0])]
bb = BangBangInterpolator(limb=arm, scale_dq=dq_scale, logfile=None,
debug=False)
# Try, for several durations, to find a solution with the bang-bang
# interpolator; if successful, store found duration in map.
durations = np.zeros((cfgs.shape[0], cfgs.shape[0]))
zeros = set_dict(arm, *(0,)*7)
times = np.arange(start=0.5, stop=15.0, step=0.15)
for n in range(cfgs.shape[0]):
for m in range(n+1, cfgs.shape[0]):
err = -1
for t in times:
_, _, err = bb.interpolate(q_start=c[n], q_end=c[m],
duration=t,
dq_start=zeros, dq_end=zeros)
if err == 0:
durations[n, m] = t
durations[m, n] = t
break
if err == -1:
raise ValueError("No valid trajectory found!")
path = raw_input(" Where to save the durations mapping: ")
print "Writing durations into '%s'." % path
np.savetxt(path, durations, delimiter=',')
return durations
def record_poses(self, path):
""" Record limb poses and corresponding configurations.
Move baxter's limb manually to a configuration in workspace and press
'y' to record the current pose and the current set of joint angles.
Press 'n' to stop recording poses.
:param path: Where to save 'poses.txt' and 'configurations.txt'.
:return: A numpy array containing the recorded poses as rows.
"""
def _endpoint_pose():
""" Current pose of the wrist of one arm of the baxter robot.
:return: pose [x, y, z, a, b, c]
"""
qp = limb.endpoint_pose()
r = transformations.euler_from_quaternion(qp['orientation'])
return [
qp['position'][0], qp['position'][1], qp['position'][2], r[0],
r[1], r[2]
]
arm = raw_input(" Record poses for 'left' or 'right' arm: ")
if arm not in ['left', 'right']:
raise ValueError("Must be 'left' or 'right' arm!")
limb = baxter_interface.Limb(arm)
poses = list()
cfgs = list()
while not rospy.is_shutdown():
key = raw_input("Record pose and configuration? (y, n): ")
if key == 'y' or key == 'Y':
print 'yes'
pose = _endpoint_pose()
cfg = limb.joint_angles()
print 'pose', pose
print 'cfg ', [cfg[jn] for jn in joint_names(arm)]
try:
cfg_ik = self._inverse_kinematics(pose, arm)
poses.append(pose)
cfgs.append(cfg_ik)
print 'ikin', list(cfg_ik)
except ValueError as e:
print "Failed to record pose due to ik failure. Repeat."
elif key == 'n' or key == 'N':
print 'Writing recorded poses and configurations ...'
np.savetxt(os.path.join(path, 'poses.txt'),
poses,
delimiter=',',
header='x y z a b c')
np.savetxt(os.path.join(path, 'configurations.txt'),
cfgs,
delimiter=',',
header='s0, s1, e0, e1, w0, w1, w2')
return np.asarray(poses)
def test_poses(self):
""" Test poses in self._data by moving through them one after the
other."""
arm = raw_input(" Test poses for 'left' or 'right' arm: ")
if arm not in ['left', 'right']:
raise ValueError("Must be 'left' or 'right' arm!")
limb = baxter_interface.Limb(arm)
for idx in range(self._data.shape[0]):
if rospy.is_shutdown():
break
cfg_ik = self._inverse_kinematics(self._data[idx, :], arm)
cmd = dict(zip(joint_names(arm), cfg_ik))
limb.move_to_joint_positions(cmd)
def test_configs(self):
""" Test configurations in self._data by moving through them one
after the other.
"""
arm = raw_input(" Test configurations for 'left' or 'right' arm: ")
if arm not in ['left', 'right']:
raise ValueError("Must be 'left' or 'right' arm!")
limb = baxter_interface.Limb(arm)
for idx in range(self._data.shape[0]):
if rospy.is_shutdown():
break
cmd = {a: b for a, b in zip(joint_names(arm), self._data[idx, :])}
limb.move_to_joint_positions(cmd)
def interpolate(self, q_start, q_end, duration, dq_start, dq_end):
""" Perform bang bang interpolation. That is, constant acceleration
from current configuration; motion with constant velocity; constant
deceleration to end at desired configuration; all optimized to arrive
there at the desired time.
:param q_start: Dictionary of joint name keys to start joint angles.
:param q_end: Dictionary of joint name keys to end joint angles.
:param duration: Duration for the motion.
:param dq_start: Dictionary of joint name keys to start joint velocities.
:param dq_end: Dictionary of joint name keys to end joint velocities.
:return: (steps, d_steps, err)
"""
length = int(np.ceil(duration * settings.interpolator_rate))
s = np.zeros((length, len(q_start)))
ds = np.zeros((length, len(q_start)))
err = 0
for idx, jn in enumerate(settings.joint_names(self._arm)):
if self._logfile:
with open(self._logfile, 'a') as fp:
fp.write('%s: ' % jn)
# turn off w2, otherwise duration will grow ridiculously long
if jn == self._arm + '_w2':
# no need to do anything, since already filled with 0s
if self._logfile:
with open(self._logfile, 'a') as fp:
fp.write('\n')
else:
s[:, idx], ds[:, idx], err = \
self._one_dof(T=duration, q0=q_start[jn], qT=q_end[jn],
dq0=dq_start[jn], dqT=dq_end[jn],
dqm=self._dq_lim[jn], ddqm=self._ddq_lim[jn])
if err == -1:
return (np.zeros((1, len(q_start))), np.zeros(
(1, len(q_start))), err)
if self._logfile:
with open(self._logfile, 'a') as fp:
fp.write('\n')
return s, ds, err
def _move_to_joint_positions(self,
des_idx,
dq_des,
kpid,
tau_lim,
anomaly_pars=None,
timeout=15.0):
""" (Blocking) PID control of robot limb with anomalies.
:param des_idx: Index of desired target configuration in list of
configurations.
:param dq_des: Dictionary of joint name keys to desired target
velocities.
:param kpid: Dictionary of joint name keys to PID control parameter
triples.
:param tau_lim: Dictionary of joint name keys to joint torque limit
tuples.
:param anomaly_pars: Anomaly parameters
[pm, im, dm, add, joint_id, a_mode, a_type] or None. If None, no
anomaly is generated, otherwise an anomaly is introduced according
to the passed parameters.
:param timeout: Timeout for motion in [s].
:return: Boolean <True, False> on completion.
"""
q_curr = self._limb.joint_angles()
dq_curr = set_dict(self._arm,
*(0.0, ) * 7) # self._limb.joint_velocities()
count = 0
rate = rospy.Rate(settings.interpolator_rate)
""" Trajectory planning """
jns = settings.joint_names(self._arm)
q_des = {a: b for a, b in zip(jns, self._configs[des_idx])}
# using closest configuration for look-up of required duration
closest_idx = self._configs.get_closest_config(
[q_curr[jn] for jn in jns])
duration = self._durations.get_duration(closest_idx, des_idx)
duration += settings.duration_offset
steps, d_steps, err = self._ipl.interpolate(q_start=q_curr,
q_end=q_des,
duration=duration,
dq_start=dq_curr,
dq_end=dq_des)
""" Trajectory execution """
if err == 0:
rospy.loginfo(
"Planned trajectory is %i-dimensional and %i steps long." %
(steps.shape[1], steps.shape[0]))
if anomaly_pars is not None:
""" Set up anomalies """
pm, im, dm, _, joint_id, _, _ = anomaly_pars
joint = jns[joint_id]
margin = 10
if settings.anomal_iters + 2 * margin > steps.shape[0]:
anomaly_iters = steps.shape[0] - 2 * margin
else:
anomaly_iters = settings.anomal_iters
anomaly_start = np.random.randint(low=margin,
high=steps.shape[0] -
anomaly_iters)
""" Set up PID controllers """
ctrl = dict()
for jn in q_curr.keys():
ctrl[jn] = PidController(kpid=kpid[jn], direction=PID_DIRECT)
ctrl[jn].set_output_limits(minmax=tau_lim[jn])
""" Do PID control """
t_elapsed = 0.0
t_start = rospy.get_time()
cmd = dict()
started = False
while (not rospy.is_shutdown() and count < steps.shape[0]
and t_elapsed < timeout):
q_curr = self._limb.joint_angles()
dq_curr = self._limb.joint_velocities()
if anomaly_pars is not None:
""" Anomaly execution """
if count >= anomaly_start and not started:
started = True
anomaly_start = count
rospy.loginfo(" Inducing anomaly on joint %s." % joint)
kp_mod = kpid[joint][0] * pm
ki_mod = kpid[joint][1] * im
kd_mod = kpid[joint][2] * dm
ctrl[joint].set_parameters(kp=kp_mod,
ki=ki_mod,
kd=kd_mod)
elif anomaly_start < count < anomaly_start + anomaly_iters:
self._pub_anom.publish(data=anomaly_pars)
elif count == anomaly_start + anomaly_iters:
ctrl[joint].set_parameters(kpid=kpid[joint])
for jn in q_curr.keys():
idx = jns.index(jn)
cmd[jn] = ctrl[jn].compute(q_curr[jn], steps[count][idx],
dq_curr[jn])
if self._joints:
command = [
steps[count][jns.index(jn)]
for jn in self._rec_joint.get_header_cfg()[1:]
]
self._pub_cfg_comm.publish(command=command)
command = [
ctrl[jn].generated
for jn in self._rec_joint.get_header_efft()[1:]
]
self._pub_efft_gen.publish(command=command)
self._limb.set_joint_torques(cmd)
rate.sleep()
t_elapsed = rospy.get_time() - t_start
count = int(np.floor(t_elapsed * settings.interpolator_rate))
if count >= steps.shape[0]:
rospy.loginfo(" Arrived at desired configuration.")
if t_elapsed >= timeout:
rospy.loginfo(" Motion timed out.")
return True
return False
def test(self):
""" Test interpolator by visual inspection. """
import matplotlib.pyplot as plt
import pprint
pp = pprint.PrettyPrinter(indent=2)
# q_start = set_dict('left', 0.0, -0.55, 0.0, 0.75, 0.0, 1.26, 0.0)
# q_start = set_dict('left', -1.22, -0.74, -0.11, 1.12, 0.14, 1.2, 3.02)
# q_start = set_dict('left', -1.23, -0.52, 0.12, 1.25, -0.04, 0.82, 3.02)
# q_start = set_dict('left', -1.16, -0.31, 0, 1.15, -0.02, 0.66, 3.05)
# q_start = set_dict('left', -1.23, -0.52, 0.12, 1.25, -0.04, 0.82, 3.02)
# q_start = set_dict('left', 0.5, -0.7, 0.15, 1.57, -0.28, 0.7, 3.05)
q_start = set_dict(self._arm, 0.5, -0.35, 0.17, 1.32, -0.33, 0.62, 3.05)
# q_start = set_dict('left', 0.5, -0.7, 0.15, 1.57, -0.28, 0.7, 3.05)
q_end = set_dict(self._arm, -0.95, -0.91, -0.14, 1.37, 0.16, 1.12, 3.03)
dq_start = set_dict(self._arm, *(0.0,)*7)
dq_end = set_dict(self._arm, *(0.0,)*7)
print 'q_start:'
pp.pprint(q_start)
print 'q_end:'
pp.pprint(q_end)
s, ds, err = self.interpolate(q_start=q_start, q_end=q_end,
duration=5.95,
dq_start=dq_start, dq_end=dq_end)
print 'returned:', err
print s.shape
if err == 0:
q_lim = settings.q_lim(self._arm)
dq_lim = settings.dq_lim(self._arm, scale=1.0)
# Visualize trajectory for ocular validation
fig, axs = plt.subplots(7, 2, figsize=(18, 32), squeeze=False)
for i, jn in enumerate(settings.joint_names(self._arm)):
axs[i][0].plot(range(s.shape[0]), s[:, i], 'k-')
axs[i][0].plot([0, s.shape[0]], [q_start[jn], q_end[jn]], 'ro')
axs[i][0].axhline(y=q_lim[jn][0], xmin=0, xmax=1,
linewidth=3, color='r', linestyle='dashed',
alpha=.3)
axs[i][0].axhline(y=q_lim[jn][1], xmin=0, xmax=1,
linewidth=3, color='r', linestyle='dashed',
alpha=.3)
axs[i][0].set_ylabel('angle [rad]')
axs[i][1].plot(range(ds.shape[0]), ds[:, i], 'g-')
axs[i][1].plot([0, ds.shape[0]], [dq_start[jn], dq_end[jn]],
'ro')
axs[i][1].axhline(y=dq_lim[jn][0], xmin=0, xmax=1,
linewidth=3, color='r', linestyle='dashed',
alpha=.3)
axs[i][1].axhline(y=dq_lim[jn][1], xmin=0, xmax=1,
linewidth=3, color='r', linestyle='dashed',
alpha=.3)
axs[i][1].set_ylabel('velocity [rad/s]')
axs[-1][0].set_xlabel('trajectory step')
axs[-1][1].set_xlabel('trajectory step')
fig.suptitle('generated trajectory from s0 (top) to w2 (bottom)')
plt.show()