def _get_r_h_configurations(self):
""" Define the inner and outer configuration dictionaries for the
robotic arm in the robot-human-handshake scenario.
:return: Two dictionaries of joint name keys to joint angle values
defining the inner and outer configurations for the robotic arm.
"""
cmd_in = set_dict(self._arm_robot, 0.12, 0.24, -1.23, 1.76, 0.09, 0.61,
1.97)
cmd_out = set_dict(self._arm_robot, 0.57, 0.24, -1.29, 1.53, 0.20,
1.32, 1.97)
return cmd_in, cmd_out
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 _get_r_r_configurations(self, mode):
""" Define the two configuration dictionaries for the robotic arm and
the 'human' arm---which in fact is another robotic arm---in the
robot-robot-handshake scenario.
:param mode: The set of configurations to return, one of
<'normal', 'occluded1', 'occluded2'>.
:return: Two dictionaries of joint name keys to joint angle values
defining the configuration for the robotic and the human arm,
respectively.
"""
if mode == 'normal':
return (set_dict(self._arm_robot, 0.02, 0.05, -1.16, 1.82, -0.09,
0.41, -0.69),
set_dict(self._arm_human, 0.29, 0.54, 0.88, 1.22, 0.45,
0.94, 0.50))
elif mode == 'occluded1':
return (set_dict(self._arm_robot, 0.08, 0.25, -1.24, 2.10, -0.39,
0.48, 1.07),
set_dict(self._arm_human, 0.29, 0.54, 0.88, 1.22, 0.45,
0.94, 0.50))
elif mode == 'occluded2':
return (set_dict(self._arm_robot, -0.12, 0.22, -0.97, 1.85, -0.79,
0.48, 0.23),
set_dict(self._arm_human, 0.40, 0.37, 0.78, 1.13, 0.69,
1.05, -0.25))
else:
raise ValueError('No such set of configurations!')
def _one_sample(self, mode='normal'):
""" One handshake sample.
Baxter moves two limbs in a velocity-controlled sine-wave or one limb
from an outer- toward an inner configuration and vice versa,
depending on self._experiment.
:param mode: A string defining which set of configurations to select.
Can be one of <'normal', 'occluded1', 'occluded2'> if self._experiment
is 'r-r', or one of <'robot', 'human'> if self._experiment is 'r-h'.
:return: True on completion.
"""
if self._experiment == 'r-r':
""" The robot-robot-handshake. The two limbs are moved to a
previously defined starting configuration and are then moved in a
velocity-controlled sinusoidal movement for a given period of
time.
"""
cmd_robot, cmd_human = self._get_r_r_configurations(mode)
command = [
cmd_robot[jn]
for jn in self._rec_joint_robot.get_header_cfg()[1:]
]
self._pub_cfg_des_robot.publish(command=command)
self._limb_robot.move_to_joint_positions(cmd_robot)
command = [
cmd_human[jn]
for jn in self._rec_joint_human.get_header_cfg()[1:]
]
self._pub_cfg_des_human.publish(command=command)
self._limb_human.move_to_joint_positions(cmd_human)
self._wobble()
self._limb_human.move_to_neutral()
else: # self._experiment == 'r-h'
""" The robot-human-handshake. If 'mode' is 'robot', the second
robotic limb is used as a placeholder for the human limb.
Otherwise it is not moved at all.
The robotic limb is moved in a periodic fashion for a given
period of time between previously defined outer- and inner
configurations, simulating a handshake (if 'mode' is 'robot') or
being grasped by the human hand otherwise.
"""
cmd_in, cmd_out = self._get_r_h_configurations()
if mode == 'robot':
cmd_human = set_dict(self._arm_human, -0.11, 0.40, 1.29, 1.62,
-0.19, 0.82, 1.28)
command = [
cmd_human[jn]
for jn in self._rec_joint_human.get_header_cfg()[1:]
]
self._pub_cfg_des_human.publish(command=command)
self._limb_human.move_to_joint_positions(cmd_human)
elif mode == 'human':
pass
else:
raise ValueError('No such configuration!')
flag_out = False
elapsed = 0.0
start = rospy.get_time()
while not rospy.is_shutdown() and elapsed < settings.run_time:
if flag_out:
cmd = cmd_in
else:
cmd = cmd_out
command = [
cmd[jn]
for jn in self._rec_joint_robot.get_header_cfg()[1:]
]
self._pub_cfg_des_robot.publish(command=command)
self._limb_robot.move_to_joint_positions(cmd)
flag_out = not flag_out
elapsed = rospy.get_time() - start
if mode == 'robot':
self._limb_human.move_to_neutral()
self._limb_robot.move_to_neutral()
return True
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()