def rpy_within_bounds(rpy, Bw): """ Checks whether an rpy value is within a given rpy bounds. Assumes all values in the bounds are [-pi, pi] Two main issues: dealing with roundoff issues for zero bounds and Wraparound for rpy. @param rpy a (3,) rpy value @param Bw bounds on rpy @return check a (3,) vector of True if within and False if outside """ # Unwrap rpy to Bw_cont. from prpy.util import wrap_to_interval rpy = wrap_to_interval(rpy, lower=Bw[:, 0]) # Check bounds condition on RPY component. rpycheck = [False] * 3 for i in range(0, 3): if (Bw[i, 0] > Bw[i, 1] + EPSILON): # An outer interval rpycheck[i] = (((rpy[i] + EPSILON) >= Bw[i, 0]) or ((rpy[i] - EPSILON) <= Bw[i, 1])) else: # An inner interval rpycheck[i] = (((rpy[i] + EPSILON) >= Bw[i, 0]) and ((rpy[i] - EPSILON) <= Bw[i, 1])) return rpycheck
def sample_xyzrpy(self, xyzrpy=NANBW): """ Samples from Bw to generate an xyzrpy sample Can specify some values optionally as NaN. @param xyzrpy (optional) a 6-vector of Bw with float('nan') for dimensions to sample uniformly. @return an xyzrpy sample """ check = self.is_valid(xyzrpy, ignoreNAN=True) if not all(check): raise ValueError('xyzrpy must be within bounds', check) Bw_sample = numpy.array([self._Bw_cont[i, 0] + (self._Bw_cont[i, 1] - self._Bw_cont[i, 0]) * numpy.random.random_sample() if numpy.isnan(x) else x for i, x in enumerate(xyzrpy)]) # Unwrap rpy to [-pi, pi] from prpy.util import wrap_to_interval Bw_sample[3:6] = wrap_to_interval(Bw_sample[3:6]) return Bw_sample
def __init__(self, T0_w=None, Tw_e=None, Bw=None, manip=None, bodyandlink='NULL'): if T0_w is None: T0_w = numpy.eye(4) if Tw_e is None: Tw_e = numpy.eye(4) if Bw is None: Bw = numpy.zeros((6, 2)) self.T0_w = numpy.array(T0_w) self.Tw_e = numpy.array(Tw_e) self.Bw = numpy.array(Bw) if numpy.any(self.Bw[0:3, 0] > self.Bw[0:3, 1]): raise ValueError('Bw translation bounds must be [min, max]', Bw) # We will now create a continuous version of the bound to maintain: # 1. Bw[i,1] > Bw[i,0] which is necessary for LBFGS-B # 2. signed rotations, necessary for expressiveness Bw_cont = numpy.copy(self.Bw) Bw_interval = Bw_cont[3:6, 1] - Bw_cont[3:6, 0] Bw_interval = numpy.minimum(Bw_interval, 2*pi) from prpy.util import wrap_to_interval Bw_cont[3:6, 0] = wrap_to_interval(Bw_cont[3:6, 0]) Bw_cont[3:6, 1] = Bw_cont[3:6, 0] + Bw_interval self._Bw_cont = Bw_cont if manip is None: self.manipindex = -1 elif type(manip) == openravepy.openravepy_int.Robot: self.manipindex = manip.GetActiveManipulatorIndex() else: self.manipindex = manip self.bodyandlink = bodyandlink