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