コード例 #1
0
ファイル: tsr.py プロジェクト: beiju/prpy
    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
コード例 #2
0
ファイル: tsr.py プロジェクト: beiju/prpy
    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
コード例 #3
0
ファイル: tsr.py プロジェクト: beiju/prpy
    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