Resolve conflict while merge fixGrabbedSelfColCheckAsym. Conflicts : …

…include/openrave/kinbody.h

include/openrave/kinbody.h

@@ -2438,7 +2438,7 @@ class OPENRAVE_API KinBody : public InterfaceBase
     typedef boost::shared_ptr<KinBodyInfo> KinBodyInfoPtr;
     typedef boost::shared_ptr<KinBodyInfo const> KinBodyInfoConstPtr;
 
-    /// \brief Alias for list of non-colliding link pairs, mostly used for Grabbed checking.
+    /// \brief Alias for list of non-colliding link pairs, mainly used for collision checking for Grabbed.
     using ListNonCollidingLinkPairs = std::list<std::pair<KinBody::LinkConstPtr, KinBody::LinkConstPtr> >;
 
     /// \brief Saved data for Grabbed used in KinBodyStateSaver and KinBodyStateSaverRef
@@ -3824,7 +3824,18 @@ class OPENRAVE_API KinBody : public InterfaceBase
     mutable std::string __hashKinematicsGeometryDynamics; ///< hash serializing kinematics, dynamics and geometry properties of the KinBody
     int64_t _lastModifiedAtUS=0; ///< us, linux epoch, last modified time of the kinbody when it was originally loaded from the environment.
     int64_t _revisionId = 0; ///< the webstack revision for this loaded kinbody
-    std::unordered_map<uint64_t, ListNonCollidingLinkPairs> _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed; ///< map of list of link pairs. This is computed when grabbed bodies are grabbed, and at taht time, two grabbed bodies are not touching each other. Since these links are not colliding at the time of grabbing, they should remain non-colliding with the grabbed body throughout. If, while grabbing, they collide with the grabbed body at some point, CheckSelfCollision should return true. It is important to note that the enable state of a link does *not* affect its membership of this list. Each pair in the list should be [Grabbed1-link, Grabbed2-link]. Note that this does not contain link pairs of [Grabbed-link, Grabber-link], c.f. Grabbed::_listNonCollidingGrabbedGrabberLinkPairsWhenGrabbed. Note that the key of this map is 'environment body indices pair', which lower 32bits are for the first KinBody's envBodyIndex, and which higher 32bits are for the second KinBody's envBodyIndex. The first envBodyIndex should be always smaller than the second envBodyIndex to simplify searching. Please also see _ComputeEnvironmentBodyIndicesPair.
+    /// _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed maps a pair of envBodyIndices of two grabbed bodies
+    /// (encoded into one uint64_t via _ComputeEnvironmentBodyIndicesPair) to a list of initially non-colliding link
+    /// pairs between the two. The ListNonCollidingLinkPairs for body1 and body2 is computed from state when the latest
+    /// body between body1 and body2 has been grabbed. Since these links in each pair are not colliding with each other
+    /// at the time of grabbing, they should remain non-colliding throughout (i.e. until either of them is released).
+    /// Notes:
+    /// - The enable states of links do *not* affect the membership of this ListNonCollidingLinkPair.
+    /// - ListNonCollidingLinkPair, which is the values of this map, only contains link pairs of *grabbed* bodies (i.e.
+    ///   not grabber's links).
+    /// - Each link pair (grabbed1Link, grabbed2Link) in ListNonCollidingLinkPair must be such that the first element
+    ///   corresponds to the grabbed body with lower environment body index.
+    std::unordered_map<uint64_t, ListNonCollidingLinkPairs> _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed;
     uint64_t _nextGrabbedBodyUniqueId = 0; ///< This indicates the unique id of the next grabbed body. Monotonically increasing, except for the resetting when all grabbed bodies are released, ...etc. Lower unique id means grabbed earlier, and we can identify the order of grabbed bodies through this Id.
 
 private:
@@ -3908,14 +3919,14 @@ class OPENRAVE_API Grabbed : public UserData, public boost::enable_shared_from_t
     const uint64_t _uniqueId = 0; ///< The unique id of this Grabbed instance. Lower unique id means grabbed earlier, and we can identify the order of grabbed bodies through this Id.
 private:
 
-    /// \brief push inter-grabbed-bodies non colliding link pairs to grabber.
+    /// \brief update grabber's _mapListNonCollidingInterGrabbedLinkPairsWhenGrabbed. if there is the existing list, push the inter-grabbed link pairs to it. otherwise, create the new list in the map and push the inter-grabbed link pairs to it.
     /// \param[out] pGrabber : updated grabber.
     /// \param[out] pchecker : collision checker
     /// \param[in] grabbedBody, otherGrabbedBody : grabbed body by this class, and other grabbed body to check.
-    void _PushNonCollidingLinkPairsForGrabbedBodies(KinBodyPtr& pGrabber,
-                                                    CollisionCheckerBasePtr& pchecker,
-                                                    const KinBody& grabbedBody,
-                                                    const KinBody& otherGrabbedBody);
+    void _UpdateMapListNonCollidingInterGrabbedLinkPairs(KinBodyPtr& pGrabber,
+                                                         CollisionCheckerBasePtr& pchecker,
+                                                         const KinBody& grabbedBody,
+                                                         const KinBody& otherGrabbedBody);
 
     bool _listNonCollidingIsValid = false; ///< a flag indicating whether the current _listNonCollidingLinksWhenGrabbed is valid or not.
     std::vector<KinBody::LinkPtr> _vAttachedToGrabbingLink; ///< vector of all links that are rigidly attached to _pGrabbingLink

sandbox/parabolicsmoother/interpolation.py

@@ -42,7 +42,7 @@ def InterpolateZeroVelND(x0Vect, x1Vect, vmVect, amVect, delta=zero):
 
     vMin = inf # the tightest velocity bound
     aMin = inf # the tightest acceleration bound
-    for i in xrange(ndof):
+    for i in range(ndof):
         if not IsEqual(x1Vect[i], x0Vect[i]):
             vMin = min(vMin, vmVect[i]/Abs(dVect[i]))
             aMin = min(aMin, amVect[i]/Abs(dVect[i]))
@@ -60,13 +60,13 @@ def InterpolateZeroVelND(x0Vect, x1Vect, vmVect, amVect, delta=zero):
         raise NotImplementedError
 
     # Scale each DOF according to the obtained sd-profile
-    curves = [ParabolicCurve() for _ in xrange(ndof)] # a list of (empty) parabolic curves
+    curves = [ParabolicCurve() for _ in range(ndof)] # a list of (empty) parabolic curves
     for sdRamp in sdProfile:
         aVect = sdRamp.a * dVect
         v0Vect = sdRamp.v0 * dVect
         dur = sdRamp.duration
 
-        for j in xrange(ndof):
+        for j in range(ndof):
             ramp = Ramp(v0Vect[j], aVect[j], dur, x0Vect[j])
             curve = ParabolicCurve([ramp])
             curves[j].Append(curve)
@@ -110,7 +110,7 @@ def InterpolateArbitraryVelND(x0Vect_, x1Vect_, v0Vect_, v1Vect_, xminVect_, xma
     curves = []
     maxDuration = zero
     maxIndex = 0
-    for i in xrange(ndof):
+    for i in range(ndof):
         if delta == zero:
             curve = Interpolate1D(x0Vect[i], x1Vect[i], v0Vect[i], v1Vect[i], vmVect[i], amVect[i])
         else:
@@ -226,7 +226,7 @@ def InterpolateNDFixedDuration(x0Vect_, x1Vect_, v0Vect_, v1Vect_, duration, xmi
     duration = ConvertFloatToMPF(duration)
 
     curves = []
-    for idof in xrange(ndof):
+    for idof in range(ndof):
         curve = Interpolate1DFixedDuration(x0Vect[idof], x1Vect[idof], v0Vect[idof], v1Vect[idof], duration, vmVect[idof], amVect[idof])
         if curve.isEmpty:
             return ParabolicCurvesND()

sandbox/parabolicsmoother/ramp.py

@@ -395,7 +395,7 @@ def Merge(self, prec=epsilon):
 
             aCur = self.ramps[0].a
             nmerged = 0 # the number of merged ramps
-            for i in xrange(1, len(self.ramps)):
+            for i in range(1, len(self.ramps)):
                 j = i - nmerged
                 if (Abs(self.ramps[j].a) > 1):
                     if Abs((Abs(mp.log10(Abs(self.ramps[j].a))) - (Abs(mp.floor(mp.log10(Abs(self.ramps[j].a))))))) < Abs((Abs(mp.log10(Abs(self.ramps[j].a))) - (Abs(mp.ceil(mp.log10(Abs(self.ramps[j].a))))))):
@@ -826,7 +826,7 @@ def GetPeaks(self):
     def _GetPeaks(self, ta, tb):
         xmin = np.zeros(self.ndof)
         xmax = np.zeros(self.ndof)
-        for i in xrange(self.ndof):
+        for i in range(self.ndof):
             xmin[i], xmax[i] = self.curves[i]._GetPeaks(ta, tb)
         return [xmin, xmax]
 
@@ -838,7 +838,7 @@ def SetConstant(self, x0Vect_, t):
 
         ndof = len(x0Vect)
         curves = []
-        for i in xrange(ndof):
+        for i in range(ndof):
             curve = ParabolicCurve()
             curve.SetConstant(x0Vect[i], t)
             curves.append(curve)
@@ -858,7 +858,7 @@ def SetSegment(self, x0Vect_, x1Vect_, v0Vect_, v1Vect_, t):
 
         ndof = len(x0Vect)
         curves = []
-        for i in xrange(ndof):
+        for i in range(ndof):
             curve = ParabolicCurve()
             curve.SetSegment(x0Vect[i], x1Vect[i], v0Vect[i], v1Vect[i], t)
             curves.append(curve)
@@ -872,7 +872,7 @@ def SetZeroDuration(self, x0Vect_, v0Vect_):
         v0Vect = ConvertFloatArrayToMPF(v0Vect_)
         ndof = len(x0Vect)
         curves = []
-        for i in xrange(ndof):
+        for i in range(ndof):
             curve = ParabolicCurve()
             curve.SetZeroDuration(x0Vect[i], v0Vect[i])
             curves.append(curve)
@@ -897,7 +897,7 @@ def Cut(self, t):
 
         leftHalf = self.curves
         rightHalf = []
-        for i in xrange(self.ndof):
+        for i in range(self.ndof):
             rightHalf.append(leftHalf[i].Cut(t))
 
         self.Initialize(leftHalf)
@@ -917,7 +917,7 @@ def TrimFront(self, t):
             return
 
         newCurves = self.curves
-        for i in xrange(self.ndof):
+        for i in range(self.ndof):
             newCurves[i].TrimFront(t)
         self.Initialize(newCurves)
         return
@@ -935,7 +935,7 @@ def TrimBack(self, t):
             return
 
         newCurves = self.curves
-        for i in xrange(self.ndof):
+        for i in range(self.ndof):
             newCurves[i].TrimBack(t)
         self.Initialize(newCurves)
         return
@@ -951,7 +951,7 @@ def PlotPos(self, fignum='Displacement Profiles', includingSW=False, dt=0.005, *
 
         xVect = [self.EvalPos(t) for t in tVect]
         plt.plot(tVect, xVect, linewidth=2, **kwargs)
-        handle = ['joint {0}'.format(i + 1) for i in xrange(self.ndof)]
+        handle = ['joint {0}'.format(i + 1) for i in range(self.ndof)]
         plt.legend(handle)
 
         if includingSW:
@@ -970,7 +970,7 @@ def PlotVel(self, fignum='Velocity Profile', includingSW=False, **kwargs):
         for curve in self.curves:
             lines.append(curve.PlotVel(fignum=fignum, **kwargs))
 
-        handles = ['joint {0}'.format(i + 1) for i in xrange(self.ndof)]
+        handles = ['joint {0}'.format(i + 1) for i in range(self.ndof)]
         plt.legend(lines, handles)
 
         if includingSW:
@@ -989,7 +989,7 @@ def PlotAcc(self, fignum='Acceleration Profiles', **kwargs):
         for curve in self.curves:
             lines.append(curve.PlotAcc(fignum=fignum, **kwargs))
 
-        handles = ['joint {0}'.format(i + 1) for i in xrange(self.ndof)]
+        handles = ['joint {0}'.format(i + 1) for i in range(self.ndof)]
         plt.legend(lines, handles)
         plt.show(False)
         return fig
@@ -1056,7 +1056,7 @@ def CheckRamps(rampsVect, xmin, xmax, vm, am):
     if not (ret == ParabolicCheckReturn.Normal):
         return ret
 
-    for i in xrange(1, len(rampsVect)):
+    for i in range(1, len(rampsVect)):
         if not FuzzyEquals(rampsVect[i - 1].v1, rampsVect[i].v0, epsilon):
             return ParabolicCheckReturn.VDiscrepancy
         ret = CheckRamp(rampsVect[i], xmin, xmax, vm, am)
@@ -1106,7 +1106,7 @@ def CheckParabolicCurvesND(curvesnd, xminVect, xmaxVect, vmVect, amVect, x0Vect,
     v1Vect_ = ConvertFloatArrayToMPF(v1Vect)
     x0Vect_ = ConvertFloatArrayToMPF(x0Vect)
     x1Vect_ = ConvertFloatArrayToMPF(x1Vect)
-    for i in xrange(curvesnd.ndof):
+    for i in range(curvesnd.ndof):
         ret = CheckParabolicCurve(curvesnd.curves[i], xminVect_[i], xmaxVect_[i], vmVect_[i], amVect_[i], x0Vect_[i], x1Vect_[i], v0Vect_[i], v1Vect_[i])
         if not (ret == ParabolicCheckReturn.Normal):
             return ret
@@ -1121,12 +1121,12 @@ def DynamicPathStringToParabolicCurvesND(dynamicpathstring):
     ndof = int(data[0])
     nlines = ndof + 2 # the number of lines containing the data for 1 ParabolicRampND
 
-    curves = [ParabolicCurve() for _ in xrange(ndof)]
+    curves = [ParabolicCurve() for _ in range(ndof)]
     nParabolicRampND = len(data)/(nlines)
 
-    for iramp in xrange(nParabolicRampND):
+    for iramp in range(nParabolicRampND):
         curoffset = iramp*nlines
-        for idof in xrange(ndof):
+        for idof in range(ndof):
             ramp1ddata = data[curoffset + 2 + idof]
             x0, v0, x1, v1, a1, v, a2, tswitch1, tswitch2, ttotal = [mp.mpf(x) for x in ramp1ddata.split(" ")]
             ramps = []
@@ -1185,14 +1185,14 @@ def ParabolicPathStringToParabolicCurvesND(parabolicpathstring):
     nchunks = len(rawdata)/nlines_chunk
 
     curvesnd = ParabolicCurvesND()
-    for ichunk in xrange(nchunks):
+    for ichunk in range(nchunks):
         curves = []
-        for idof in xrange(ndof):
+        for idof in range(ndof):
             curvedata = rawdata[(ichunk*nlines_chunk) + 2 + idof]
             curvedata = curvedata.strip().split(" ")
             curve = ParabolicCurve()
             nramps = len(curvedata)/4
-            for iramp in xrange(nramps):
+            for iramp in range(nramps):
                 v, a, t, x0 = [float(dummy) for dummy in curvedata[(iramp*4):((iramp + 1)*4)]]
                 ramp = Ramp(v, a, t, x0)
                 nextCurve = ParabolicCurve([ramp])
@@ -1242,12 +1242,12 @@ def GetSpecificChunkFromParabolicPathString(parabolicpathstring, chunkindex):
     nchunks = len(rawdata)/nlines_chunk
 
     curves = []
-    for idof in xrange(ndof):
+    for idof in range(ndof):
         curvedata = rawdata[(chunkindex*nlines_chunk) + 2 + idof]
         curvedata = curvedata.strip().split(" ")
         curve = ParabolicCurve()
         nramps = len(curvedata)/4
-        for iramp in xrange(nramps):
+        for iramp in range(nramps):
             v, a, t, x0 = [float(dummy) for dummy in curvedata[(iramp*4):((iramp + 1)*4)]]
             ramp = Ramp(v, a, t, x0)
             nextCurve = ParabolicCurve([ramp])
@@ -1278,10 +1278,10 @@ def ConvertNewParabolicPathStringToParabolicCurvesND(parabolicpathstring):
 
     # check soundness
     ndof = int(rawdata[0].strip().split(" ")[0])
-    for i in xrange(nrampnds):
+    for i in range(nrampnds):
         assert( ndof == int((len(rawdata[i].strip().split(" ")) - 2)/5) )
 
-    for i in xrange(nrampnds):
+    for i in range(nrampnds):
         data = rawdata[i].strip().split(" ")
         data = [float(x) for x in data[1:]]
         offset = 0
@@ -1321,7 +1321,7 @@ def ConvertOpenRAVETrajectoryToParabolicCurvesND(traj):
     deltatimegroup = spec.GetGroupFromName('deltatime')
     toffset = deltatimegroup.offset
 
-    for iwaypoint in xrange(nwaypoints - 1):
+    for iwaypoint in range(nwaypoints - 1):
         x0 = traj.GetWaypoint(iwaypoint)[iwaypoint*xoffset: iwaypoint*xoffset + xdof]
         x1 = traj.GetWaypoint(iwaypoint + 1)[xoffset: xoffset + xdof]
         v0 = traj.GetWaypoint(iwaypoint)[voffset: voffset + vdof]
@@ -1343,7 +1343,7 @@ def ConvertNewParabolicPathStringToOpenRAVETrajectory(env, robot, parabolicpaths
 
     # check soundness
     ndof = int(rawdata[0].strip().split(" ")[0])
-    for i in xrange(nrampnds):
+    for i in range(nrampnds):
         assert( ndof == int((len(rawdata[i].strip().split(" ")) - 2)/5) )
     assert( robot.GetActiveDOF() == ndof )
 
@@ -1371,7 +1371,7 @@ def ConvertNewParabolicPathStringToOpenRAVETrajectory(env, robot, parabolicpaths
     waypoint = np.hstack([x0, v0, 0])
     traj.Insert(0, waypoint)
 
-    for i in xrange(nrampnds):
+    for i in range(nrampnds):
         data = rawdata[i].strip().split(" ")
         data = [float(x) for x in data[1:]]
         offset = 0
@@ -1414,7 +1414,7 @@ def ConvertParabolicCurvesNDToOpenRAVETrajectory(env, robot, curvesnd):
     traj.Insert(traj.GetNumWaypoints(), vtrajpoint)
     tprev = 0
 
-    for iswitchpoint in xrange(1, len(curvesnd.switchpointsList)):
+    for iswitchpoint in range(1, len(curvesnd.switchpointsList)):
         t = curvesnd.switchpointsList[iswitchpoint]
         deltatime = t - tprev
         q = curvesnd.EvalPos(t)