From 22e86544d23481b3676dde8913ad7e0b12e4e3a5 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Fri, 1 Nov 2024 15:21:46 +0900 Subject: [PATCH 01/10] xrange->range --- sandbox/parabolicsmoother/interpolation.py | 10 ++-- sandbox/parabolicsmoother/ramp.py | 54 +++++++++++----------- 2 files changed, 32 insertions(+), 32 deletions(-) diff --git a/sandbox/parabolicsmoother/interpolation.py b/sandbox/parabolicsmoother/interpolation.py index d4825922b7..b939b8bf11 100644 --- a/sandbox/parabolicsmoother/interpolation.py +++ b/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() diff --git a/sandbox/parabolicsmoother/ramp.py b/sandbox/parabolicsmoother/ramp.py index 978ff8a7f9..e5b367f271 100644 --- a/sandbox/parabolicsmoother/ramp.py +++ b/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) From 9145b9d3c58ff8d76c196ada7ad98e9eece707d0 Mon Sep 17 00:00:00 2001 From: Leonid Terenin Date: Tue, 5 Nov 2024 20:04:50 +0900 Subject: [PATCH 02/10] Exception / ASSERT confusion --- python/bindings/openravepy_kinbody.cpp | 68 +++++++++++++------------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 8b4ef9ffc0..d0d6f2385d 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -872,35 +872,35 @@ JointControlInfo_RobotControllerPtr PyJointControlInfo_RobotController::GetJoint info.controllerType = controllerType; if( !IS_PYTHONOBJECT_NONE(robotControllerAxisIndex) ) { size_t num = len(robotControllerAxisIndex); - OPENRAVE_EXCEPTION_FORMAT0(num == info.robotControllerAxisIndex.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisIndex.size(), "unexpected size", ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisIndex[i] = py::extract(robotControllerAxisIndex[py::to_object(i)]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisMult) ) { size_t num = len(robotControllerAxisMult); - OPENRAVE_EXCEPTION_FORMAT0(num == info.robotControllerAxisMult.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisMult.size(), "unexpected size", ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisMult[i] = py::extract(robotControllerAxisMult[py::to_object(i)]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisOffset) ) { size_t num = len(robotControllerAxisOffset); - OPENRAVE_EXCEPTION_FORMAT0(num == info.robotControllerAxisOffset.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisOffset.size(), "unexpected size", ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisOffset[i] = py::extract(robotControllerAxisOffset[py::to_object(i)]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisManufacturerCode) ) { size_t num = len(robotControllerAxisManufacturerCode); - OPENRAVE_EXCEPTION_FORMAT0(num == info.robotControllerAxisManufacturerCode.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisManufacturerCode.size(), "unexpected size", ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisManufacturerCode[i] = py::extract(robotControllerAxisManufacturerCode[i]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisProductCode) ) { size_t num = len(robotControllerAxisProductCode); - OPENRAVE_EXCEPTION_FORMAT0(num == info.robotControllerAxisProductCode.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisProductCode.size(), "unexpected size", ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisProductCode[i] = py::extract(robotControllerAxisProductCode[i]); } @@ -1001,7 +1001,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() size_t num1, num2; if( !IS_PYTHONOBJECT_NONE(moveIONames) ) { num1 = len(moveIONames); - OPENRAVE_EXCEPTION_FORMAT0(num1 == info.moveIONames.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 == info.moveIONames.size(), "unexpected size", ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(moveIONames[py::to_object(i1)])); @@ -1017,7 +1017,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(upperLimitIONames) ) { num1 = len(upperLimitIONames); - OPENRAVE_EXCEPTION_FORMAT0(num1 == info.upperLimitIONames.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 == info.upperLimitIONames.size(), "unexpected size", ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(upperLimitIONames[py::to_object(i1)])); @@ -1033,7 +1033,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(upperLimitSensorIsOn) ) { num1 = len(upperLimitSensorIsOn); - OPENRAVE_EXCEPTION_FORMAT0(num1 == info.upperLimitSensorIsOn.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 == info.upperLimitSensorIsOn.size(), "unexpected size", ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(upperLimitSensorIsOn[py::to_object(i1)])); @@ -1049,7 +1049,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(lowerLimitIONames) ) { num1 = len(lowerLimitIONames); - OPENRAVE_EXCEPTION_FORMAT0(num1 == info.lowerLimitIONames.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 == info.lowerLimitIONames.size(), "unexpected size", ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(lowerLimitIONames[py::to_object(i1)])); @@ -1065,7 +1065,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(lowerLimitSensorIsOn) ) { num1 = len(lowerLimitSensorIsOn); - OPENRAVE_EXCEPTION_FORMAT0(num1 == info.lowerLimitSensorIsOn.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 == info.lowerLimitSensorIsOn.size(), "unexpected size", ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(lowerLimitSensorIsOn[py::to_object(i1)])); @@ -1222,7 +1222,7 @@ KinBody::JointInfoPtr PyJointInfo::GetJointInfo() { // We might be able to replace these exceptions with static_assert in C++11 size_t num = len(_vaxes); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vaxes.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vaxes.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vaxes[i] = ExtractVector3(_vaxes[py::to_object(i)]); } @@ -1232,79 +1232,79 @@ KinBody::JointInfoPtr PyJointInfo::GetJointInfo() { } num = len(_vresolution); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vresolution.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vresolution.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vresolution[i] = py::extract(_vresolution[py::to_object(i)]); } num = len(_vmaxvel); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vmaxvel.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vmaxvel.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxvel[i] = py::extract(_vmaxvel[py::to_object(i)]); } num = len(_vhardmaxvel); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vhardmaxvel.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxvel.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vhardmaxvel[i] = py::extract(_vhardmaxvel[py::to_object(i)]); } num = len(_vmaxaccel); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vmaxaccel.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vmaxaccel.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxaccel[i] = py::extract(_vmaxaccel[py::to_object(i)]); } num = len(_vhardmaxaccel); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vhardmaxaccel.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxaccel.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vhardmaxaccel[i] = py::extract(_vhardmaxaccel[py::to_object(i)]); } num = len(_vmaxjerk); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vmaxjerk.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vmaxjerk.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxjerk[i] = py::extract(_vmaxjerk[py::to_object(i)]); } num = len(_vhardmaxjerk); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vhardmaxjerk.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxjerk.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vhardmaxjerk[i] = py::extract(_vhardmaxjerk[py::to_object(i)]); } num = len(_vmaxtorque); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vmaxtorque.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vmaxtorque.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxtorque[i] = py::extract(_vmaxtorque[py::to_object(i)]); } num = len(_vmaxinertia); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vmaxinertia.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vmaxinertia.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxinertia[i] = py::extract(_vmaxinertia[py::to_object(i)]); } num = len(_vweights); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vweights.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vweights.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vweights[i] = py::extract(_vweights[py::to_object(i)]); } num = len(_voffsets); - OPENRAVE_EXCEPTION_FORMAT0(num == info._voffsets.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._voffsets.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._voffsets[i] = py::extract(_voffsets[py::to_object(i)]); } num = len(_vlowerlimit); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vlowerlimit.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vlowerlimit.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vlowerlimit[i] = py::extract(_vlowerlimit[py::to_object(i)]); } num = len(_vupperlimit); - OPENRAVE_EXCEPTION_FORMAT0(num == info._vupperlimit.size(), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vupperlimit.size(), "unexpected size", ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vupperlimit[i] = py::extract(_vupperlimit[py::to_object(i)]); } @@ -1847,7 +1847,7 @@ void PyLink::InitGeometries(object ogeometryinfos) for(size_t i = 0; i < geometries.size(); ++i) { PyGeometryInfoPtr pygeom = py::extract(ogeometryinfos[py::to_object(i)]); if( !pygeom ) { - throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } geometries[i] = pygeom->GetGeometryInfo(); } @@ -1858,7 +1858,7 @@ void PyLink::AddGeometry(object ogeometryinfo, bool addToGroups) { PyGeometryInfoPtr pygeom = py::extract(ogeometryinfo); if( !pygeom ) { - throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } _plink->AddGeometry(pygeom->GetGeometryInfo(), addToGroups); } @@ -1867,7 +1867,7 @@ void PyLink::AddGeometryToGroup(object ogeometryinfo, const std::string& groupna { PyGeometryInfoPtr pygeom = py::extract(ogeometryinfo); if( !pygeom ) { - throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } _plink->AddGeometryToGroup(pygeom->GetGeometryInfo(), groupname); } @@ -1897,7 +1897,7 @@ void PyLink::SetGroupGeometries(const std::string& name, object ogeometryinfos) for(size_t i = 0; i < geometries.size(); ++i) { PyGeometryInfoPtr pygeom = py::extract(ogeometryinfos[py::to_object(i)]); if( !pygeom ) { - throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } geometries[i] = pygeom->GetGeometryInfo(); } @@ -2844,7 +2844,7 @@ bool PyKinBody::InitFromGeometries(object ogeometries, const std::string& uri) for(size_t i = 0; i < geometries.size(); ++i) { PyGeometryInfoPtr pygeom = py::extract(ogeometries[py::to_object(i)]); if( !pygeom ) { - throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } geometries[i] = *pygeom->GetGeometryInfo(); } @@ -2857,7 +2857,7 @@ void PyKinBody::InitFromLinkInfos(py::object olinkinfos, const std::string& uri) for(size_t i = 0; i < linkInfos.size(); ++i) { PyLinkInfoPtr pylinkinfo = py::extract(olinkinfos[py::to_object(i)]); if( !pylinkinfo ) { - throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.LinkInfo"),ORE_InvalidArguments); + throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.LinkInfo"),ORE_InvalidArguments); } linkInfos[i] = *pylinkinfo->GetLinkInfo(); } @@ -2888,7 +2888,7 @@ void PyKinBody::SetLinkGroupGeometries(const std::string& geomname, object olink for(size_t j = 0; j < geometries.size(); ++j) { PyGeometryInfoPtr pygeom = py::extract(infoi[py::to_object(j)]); if( !pygeom ) { - throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } geometries[j] = pygeom->GetGeometryInfo(); } @@ -2902,7 +2902,7 @@ void PyKinBody::_ParseLinkInfos(object olinkinfos, std::vector(olinkinfos[py::to_object(i)]); if( !pylink ) { - throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.LinkInfo"),ORE_InvalidArguments); + throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.LinkInfo"),ORE_InvalidArguments); } vlinkinfos[i] = pylink->GetLinkInfo(); } @@ -2914,7 +2914,7 @@ void PyKinBody::_ParseJointInfos(object ojointinfos, std::vector(ojointinfos[py::to_object(i)]); if( !pyjoint ) { - throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.JointInfo"),ORE_InvalidArguments); + throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.JointInfo"),ORE_InvalidArguments); } vjointinfos[i] = pyjoint->GetJointInfo(); } @@ -4218,7 +4218,7 @@ void PyKinBody::ResetGrabbed(object ograbbedinfos) for(size_t i = 0; i < vgrabbedinfos.size(); ++i) { PyGrabbedInfoPtr pygrabbed = py::extract(ograbbedinfos[py::to_object(i)]); if( !pygrabbed ) { - throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to Robot.GrabbedInfo"),ORE_InvalidArguments); + throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to Robot.GrabbedInfo"),ORE_InvalidArguments); } vgrabbedinfos[i] = pygrabbed->GetGrabbedInfo(); } From b1ea700e1b516bc2f9a322b43b90ba1611cbc53f Mon Sep 17 00:00:00 2001 From: Leonid Terenin Date: Tue, 5 Nov 2024 20:08:17 +0900 Subject: [PATCH 03/10] typo fix --- python/bindings/openravepy_kinbody.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index d0d6f2385d..600a75f338 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -1847,7 +1847,7 @@ void PyLink::InitGeometries(object ogeometryinfos) for(size_t i = 0; i < geometries.size(); ++i) { PyGeometryInfoPtr pygeom = py::extract(ogeometryinfos[py::to_object(i)]); if( !pygeom ) { - throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } geometries[i] = pygeom->GetGeometryInfo(); } @@ -1858,7 +1858,7 @@ void PyLink::AddGeometry(object ogeometryinfo, bool addToGroups) { PyGeometryInfoPtr pygeom = py::extract(ogeometryinfo); if( !pygeom ) { - throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } _plink->AddGeometry(pygeom->GetGeometryInfo(), addToGroups); } @@ -1867,7 +1867,7 @@ void PyLink::AddGeometryToGroup(object ogeometryinfo, const std::string& groupna { PyGeometryInfoPtr pygeom = py::extract(ogeometryinfo); if( !pygeom ) { - throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } _plink->AddGeometryToGroup(pygeom->GetGeometryInfo(), groupname); } @@ -1897,7 +1897,7 @@ void PyLink::SetGroupGeometries(const std::string& name, object ogeometryinfos) for(size_t i = 0; i < geometries.size(); ++i) { PyGeometryInfoPtr pygeom = py::extract(ogeometryinfos[py::to_object(i)]); if( !pygeom ) { - throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } geometries[i] = pygeom->GetGeometryInfo(); } @@ -2844,7 +2844,7 @@ bool PyKinBody::InitFromGeometries(object ogeometries, const std::string& uri) for(size_t i = 0; i < geometries.size(); ++i) { PyGeometryInfoPtr pygeom = py::extract(ogeometries[py::to_object(i)]); if( !pygeom ) { - throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } geometries[i] = *pygeom->GetGeometryInfo(); } @@ -2857,7 +2857,7 @@ void PyKinBody::InitFromLinkInfos(py::object olinkinfos, const std::string& uri) for(size_t i = 0; i < linkInfos.size(); ++i) { PyLinkInfoPtr pylinkinfo = py::extract(olinkinfos[py::to_object(i)]); if( !pylinkinfo ) { - throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.LinkInfo"),ORE_InvalidArguments); + throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.LinkInfo"),ORE_InvalidArguments); } linkInfos[i] = *pylinkinfo->GetLinkInfo(); } @@ -2888,7 +2888,7 @@ void PyKinBody::SetLinkGroupGeometries(const std::string& geomname, object olink for(size_t j = 0; j < geometries.size(); ++j) { PyGeometryInfoPtr pygeom = py::extract(infoi[py::to_object(j)]); if( !pygeom ) { - throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); + throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments); } geometries[j] = pygeom->GetGeometryInfo(); } @@ -2902,7 +2902,7 @@ void PyKinBody::_ParseLinkInfos(object olinkinfos, std::vector(olinkinfos[py::to_object(i)]); if( !pylink ) { - throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.LinkInfo"),ORE_InvalidArguments); + throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.LinkInfo"),ORE_InvalidArguments); } vlinkinfos[i] = pylink->GetLinkInfo(); } @@ -2914,7 +2914,7 @@ void PyKinBody::_ParseJointInfos(object ojointinfos, std::vector(ojointinfos[py::to_object(i)]); if( !pyjoint ) { - throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to KinBody.JointInfo"),ORE_InvalidArguments); + throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.JointInfo"),ORE_InvalidArguments); } vjointinfos[i] = pyjoint->GetJointInfo(); } @@ -4218,7 +4218,7 @@ void PyKinBody::ResetGrabbed(object ograbbedinfos) for(size_t i = 0; i < vgrabbedinfos.size(); ++i) { PyGrabbedInfoPtr pygrabbed = py::extract(ograbbedinfos[py::to_object(i)]); if( !pygrabbed ) { - throw OPENRAVE_ASSERT_FORMAT0(_("cannot cast to Robot.GrabbedInfo"),ORE_InvalidArguments); + throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to Robot.GrabbedInfo"),ORE_InvalidArguments); } vgrabbedinfos[i] = pygrabbed->GetGrabbedInfo(); } From b875dccd41942aca0fdef9329e6ae3af8ed49085 Mon Sep 17 00:00:00 2001 From: Leonid Terenin Date: Tue, 5 Nov 2024 20:09:11 +0900 Subject: [PATCH 04/10] i18n fix --- python/bindings/openravepy_kinbody.cpp | 48 +++++++++++++------------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 600a75f338..8b2317fdbe 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -872,35 +872,35 @@ JointControlInfo_RobotControllerPtr PyJointControlInfo_RobotController::GetJoint info.controllerType = controllerType; if( !IS_PYTHONOBJECT_NONE(robotControllerAxisIndex) ) { size_t num = len(robotControllerAxisIndex); - OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisIndex.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisIndex.size(), _("unexpected size"), ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisIndex[i] = py::extract(robotControllerAxisIndex[py::to_object(i)]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisMult) ) { size_t num = len(robotControllerAxisMult); - OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisMult.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisMult.size(), _("unexpected size"), ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisMult[i] = py::extract(robotControllerAxisMult[py::to_object(i)]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisOffset) ) { size_t num = len(robotControllerAxisOffset); - OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisOffset.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisOffset.size(), _("unexpected size"), ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisOffset[i] = py::extract(robotControllerAxisOffset[py::to_object(i)]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisManufacturerCode) ) { size_t num = len(robotControllerAxisManufacturerCode); - OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisManufacturerCode.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisManufacturerCode.size(), _("unexpected size"), ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisManufacturerCode[i] = py::extract(robotControllerAxisManufacturerCode[i]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisProductCode) ) { size_t num = len(robotControllerAxisProductCode); - OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisProductCode.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisProductCode.size(), _("unexpected size"), ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisProductCode[i] = py::extract(robotControllerAxisProductCode[i]); } @@ -1001,7 +1001,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() size_t num1, num2; if( !IS_PYTHONOBJECT_NONE(moveIONames) ) { num1 = len(moveIONames); - OPENRAVE_ASSERT_FORMAT0(num1 == info.moveIONames.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 == info.moveIONames.size(), _("unexpected size"), ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(moveIONames[py::to_object(i1)])); @@ -1017,7 +1017,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(upperLimitIONames) ) { num1 = len(upperLimitIONames); - OPENRAVE_ASSERT_FORMAT0(num1 == info.upperLimitIONames.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 == info.upperLimitIONames.size(), _("unexpected size"), ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(upperLimitIONames[py::to_object(i1)])); @@ -1033,7 +1033,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(upperLimitSensorIsOn) ) { num1 = len(upperLimitSensorIsOn); - OPENRAVE_ASSERT_FORMAT0(num1 == info.upperLimitSensorIsOn.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 == info.upperLimitSensorIsOn.size(), _("unexpected size"), ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(upperLimitSensorIsOn[py::to_object(i1)])); @@ -1049,7 +1049,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(lowerLimitIONames) ) { num1 = len(lowerLimitIONames); - OPENRAVE_ASSERT_FORMAT0(num1 == info.lowerLimitIONames.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 == info.lowerLimitIONames.size(), _("unexpected size"), ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(lowerLimitIONames[py::to_object(i1)])); @@ -1065,7 +1065,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(lowerLimitSensorIsOn) ) { num1 = len(lowerLimitSensorIsOn); - OPENRAVE_ASSERT_FORMAT0(num1 == info.lowerLimitSensorIsOn.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 == info.lowerLimitSensorIsOn.size(), _("unexpected size"), ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(lowerLimitSensorIsOn[py::to_object(i1)])); @@ -1222,7 +1222,7 @@ KinBody::JointInfoPtr PyJointInfo::GetJointInfo() { // We might be able to replace these exceptions with static_assert in C++11 size_t num = len(_vaxes); - OPENRAVE_ASSERT_FORMAT0(num == info._vaxes.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vaxes.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vaxes[i] = ExtractVector3(_vaxes[py::to_object(i)]); } @@ -1232,79 +1232,79 @@ KinBody::JointInfoPtr PyJointInfo::GetJointInfo() { } num = len(_vresolution); - OPENRAVE_ASSERT_FORMAT0(num == info._vresolution.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vresolution.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vresolution[i] = py::extract(_vresolution[py::to_object(i)]); } num = len(_vmaxvel); - OPENRAVE_ASSERT_FORMAT0(num == info._vmaxvel.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vmaxvel.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxvel[i] = py::extract(_vmaxvel[py::to_object(i)]); } num = len(_vhardmaxvel); - OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxvel.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxvel.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vhardmaxvel[i] = py::extract(_vhardmaxvel[py::to_object(i)]); } num = len(_vmaxaccel); - OPENRAVE_ASSERT_FORMAT0(num == info._vmaxaccel.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vmaxaccel.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxaccel[i] = py::extract(_vmaxaccel[py::to_object(i)]); } num = len(_vhardmaxaccel); - OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxaccel.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxaccel.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vhardmaxaccel[i] = py::extract(_vhardmaxaccel[py::to_object(i)]); } num = len(_vmaxjerk); - OPENRAVE_ASSERT_FORMAT0(num == info._vmaxjerk.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vmaxjerk.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxjerk[i] = py::extract(_vmaxjerk[py::to_object(i)]); } num = len(_vhardmaxjerk); - OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxjerk.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxjerk.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vhardmaxjerk[i] = py::extract(_vhardmaxjerk[py::to_object(i)]); } num = len(_vmaxtorque); - OPENRAVE_ASSERT_FORMAT0(num == info._vmaxtorque.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vmaxtorque.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxtorque[i] = py::extract(_vmaxtorque[py::to_object(i)]); } num = len(_vmaxinertia); - OPENRAVE_ASSERT_FORMAT0(num == info._vmaxinertia.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vmaxinertia.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxinertia[i] = py::extract(_vmaxinertia[py::to_object(i)]); } num = len(_vweights); - OPENRAVE_ASSERT_FORMAT0(num == info._vweights.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vweights.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vweights[i] = py::extract(_vweights[py::to_object(i)]); } num = len(_voffsets); - OPENRAVE_ASSERT_FORMAT0(num == info._voffsets.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._voffsets.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._voffsets[i] = py::extract(_voffsets[py::to_object(i)]); } num = len(_vlowerlimit); - OPENRAVE_ASSERT_FORMAT0(num == info._vlowerlimit.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vlowerlimit.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vlowerlimit[i] = py::extract(_vlowerlimit[py::to_object(i)]); } num = len(_vupperlimit); - OPENRAVE_ASSERT_FORMAT0(num == info._vupperlimit.size(), "unexpected size", ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vupperlimit.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vupperlimit[i] = py::extract(_vupperlimit[py::to_object(i)]); } From 2bcac7723fa489fc0cf5da89e6635d810a227026 Mon Sep 17 00:00:00 2001 From: Leonid Terenin Date: Wed, 6 Nov 2024 11:44:35 +0900 Subject: [PATCH 05/10] more detailed debug info --- python/bindings/openravepy_kinbody.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 8b2317fdbe..3bfd6719fd 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -1222,7 +1222,9 @@ KinBody::JointInfoPtr PyJointInfo::GetJointInfo() { // We might be able to replace these exceptions with static_assert in C++11 size_t num = len(_vaxes); - OPENRAVE_ASSERT_FORMAT0(num == info._vaxes.size(), _("unexpected size"), ORE_InvalidState); + std::string msg( boost::str(boost::format("unexpected size, received: %d, expected: %d") % num % info._vaxes.size())); +// OPENRAVE_ASSERT_FORMAT0(num == info._vaxes.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num == info._vaxes.size(), msg.c_str(), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vaxes[i] = ExtractVector3(_vaxes[py::to_object(i)]); } From f5214a27a85fb16eef3a2a314b66c93aeed18686 Mon Sep 17 00:00:00 2001 From: Leonid Terenin Date: Wed, 6 Nov 2024 14:08:27 +0900 Subject: [PATCH 06/10] comment out "_vaxes.size()" check --- python/bindings/openravepy_kinbody.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 3bfd6719fd..2d9b4de09e 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -1222,9 +1222,12 @@ KinBody::JointInfoPtr PyJointInfo::GetJointInfo() { // We might be able to replace these exceptions with static_assert in C++11 size_t num = len(_vaxes); - std::string msg( boost::str(boost::format("unexpected size, received: %d, expected: %d") % num % info._vaxes.size())); + + // possible bug : info._vaxes.size() is fixed to 3 as "boost::array _vaxes", so even for + // "hinge" we could not get 1, so commenting this ASSERT out until there's a better solution because + // right now it trips `test_kinematics : test_custombody` with hinge joint (len(_vaxes) == 1) + // OPENRAVE_ASSERT_FORMAT0(num == info._vaxes.size(), _("unexpected size"), ORE_InvalidState); - OPENRAVE_ASSERT_FORMAT0(num == info._vaxes.size(), msg.c_str(), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vaxes[i] = ExtractVector3(_vaxes[py::to_object(i)]); } From f6750df2d885dc37dbd0a4275fd6a82ad3430493 Mon Sep 17 00:00:00 2001 From: Leonid Terenin Date: Wed, 6 Nov 2024 15:50:00 +0900 Subject: [PATCH 07/10] comment out "_vlower/upperlimit.size()" check --- python/bindings/openravepy_kinbody.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 2d9b4de09e..2979ed0b24 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -1303,13 +1303,13 @@ KinBody::JointInfoPtr PyJointInfo::GetJointInfo() { } num = len(_vlowerlimit); - OPENRAVE_ASSERT_FORMAT0(num == info._vlowerlimit.size(), _("unexpected size"), ORE_InvalidState); + // OPENRAVE_ASSERT_FORMAT0(num == info._vlowerlimit.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vlowerlimit[i] = py::extract(_vlowerlimit[py::to_object(i)]); } num = len(_vupperlimit); - OPENRAVE_ASSERT_FORMAT0(num == info._vupperlimit.size(), _("unexpected size"), ORE_InvalidState); + // OPENRAVE_ASSERT_FORMAT0(num == info._vupperlimit.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vupperlimit[i] = py::extract(_vupperlimit[py::to_object(i)]); } From f658435ca4752691d7c60eacea4c25b230e2297a Mon Sep 17 00:00:00 2001 From: Leonid Terenin Date: Wed, 6 Nov 2024 16:10:03 +0900 Subject: [PATCH 08/10] change to "smaller-or-equal" --- python/bindings/openravepy_kinbody.cpp | 33 +++++++++++--------------- 1 file changed, 14 insertions(+), 19 deletions(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 2979ed0b24..09c2702522 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -1222,12 +1222,7 @@ KinBody::JointInfoPtr PyJointInfo::GetJointInfo() { // We might be able to replace these exceptions with static_assert in C++11 size_t num = len(_vaxes); - - // possible bug : info._vaxes.size() is fixed to 3 as "boost::array _vaxes", so even for - // "hinge" we could not get 1, so commenting this ASSERT out until there's a better solution because - // right now it trips `test_kinematics : test_custombody` with hinge joint (len(_vaxes) == 1) - -// OPENRAVE_ASSERT_FORMAT0(num == info._vaxes.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vaxes.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vaxes[i] = ExtractVector3(_vaxes[py::to_object(i)]); } @@ -1237,79 +1232,79 @@ KinBody::JointInfoPtr PyJointInfo::GetJointInfo() { } num = len(_vresolution); - OPENRAVE_ASSERT_FORMAT0(num == info._vresolution.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vresolution.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vresolution[i] = py::extract(_vresolution[py::to_object(i)]); } num = len(_vmaxvel); - OPENRAVE_ASSERT_FORMAT0(num == info._vmaxvel.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vmaxvel.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxvel[i] = py::extract(_vmaxvel[py::to_object(i)]); } num = len(_vhardmaxvel); - OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxvel.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vhardmaxvel.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vhardmaxvel[i] = py::extract(_vhardmaxvel[py::to_object(i)]); } num = len(_vmaxaccel); - OPENRAVE_ASSERT_FORMAT0(num == info._vmaxaccel.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vmaxaccel.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxaccel[i] = py::extract(_vmaxaccel[py::to_object(i)]); } num = len(_vhardmaxaccel); - OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxaccel.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vhardmaxaccel.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vhardmaxaccel[i] = py::extract(_vhardmaxaccel[py::to_object(i)]); } num = len(_vmaxjerk); - OPENRAVE_ASSERT_FORMAT0(num == info._vmaxjerk.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vmaxjerk.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxjerk[i] = py::extract(_vmaxjerk[py::to_object(i)]); } num = len(_vhardmaxjerk); - OPENRAVE_ASSERT_FORMAT0(num == info._vhardmaxjerk.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vhardmaxjerk.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vhardmaxjerk[i] = py::extract(_vhardmaxjerk[py::to_object(i)]); } num = len(_vmaxtorque); - OPENRAVE_ASSERT_FORMAT0(num == info._vmaxtorque.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vmaxtorque.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxtorque[i] = py::extract(_vmaxtorque[py::to_object(i)]); } num = len(_vmaxinertia); - OPENRAVE_ASSERT_FORMAT0(num == info._vmaxinertia.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vmaxinertia.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vmaxinertia[i] = py::extract(_vmaxinertia[py::to_object(i)]); } num = len(_vweights); - OPENRAVE_ASSERT_FORMAT0(num == info._vweights.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vweights.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vweights[i] = py::extract(_vweights[py::to_object(i)]); } num = len(_voffsets); - OPENRAVE_ASSERT_FORMAT0(num == info._voffsets.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._voffsets.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._voffsets[i] = py::extract(_voffsets[py::to_object(i)]); } num = len(_vlowerlimit); - // OPENRAVE_ASSERT_FORMAT0(num == info._vlowerlimit.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vlowerlimit.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vlowerlimit[i] = py::extract(_vlowerlimit[py::to_object(i)]); } num = len(_vupperlimit); - // OPENRAVE_ASSERT_FORMAT0(num == info._vupperlimit.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info._vupperlimit.size(), _("unexpected size"), ORE_InvalidState); for(size_t i = 0; i < num; ++i) { info._vupperlimit[i] = py::extract(_vupperlimit[py::to_object(i)]); } From 013509e22d1a8e32b07354854171245ebb6bc60c Mon Sep 17 00:00:00 2001 From: Leonid Terenin Date: Wed, 6 Nov 2024 17:30:57 +0900 Subject: [PATCH 09/10] change to "smaller-or-equal", take 2 --- python/bindings/openravepy_kinbody.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 09c2702522..3e4ef1b850 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -872,35 +872,35 @@ JointControlInfo_RobotControllerPtr PyJointControlInfo_RobotController::GetJoint info.controllerType = controllerType; if( !IS_PYTHONOBJECT_NONE(robotControllerAxisIndex) ) { size_t num = len(robotControllerAxisIndex); - OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisIndex.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info.robotControllerAxisIndex.size(), _("unexpected size"), ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisIndex[i] = py::extract(robotControllerAxisIndex[py::to_object(i)]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisMult) ) { size_t num = len(robotControllerAxisMult); - OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisMult.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info.robotControllerAxisMult.size(), _("unexpected size"), ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisMult[i] = py::extract(robotControllerAxisMult[py::to_object(i)]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisOffset) ) { size_t num = len(robotControllerAxisOffset); - OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisOffset.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info.robotControllerAxisOffset.size(), _("unexpected size"), ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisOffset[i] = py::extract(robotControllerAxisOffset[py::to_object(i)]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisManufacturerCode) ) { size_t num = len(robotControllerAxisManufacturerCode); - OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisManufacturerCode.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info.robotControllerAxisManufacturerCode.size(), _("unexpected size"), ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisManufacturerCode[i] = py::extract(robotControllerAxisManufacturerCode[i]); } } if( !IS_PYTHONOBJECT_NONE(robotControllerAxisProductCode) ) { size_t num = len(robotControllerAxisProductCode); - OPENRAVE_ASSERT_FORMAT0(num == info.robotControllerAxisProductCode.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num <= info.robotControllerAxisProductCode.size(), _("unexpected size"), ORE_InvalidState); for( size_t i = 0; i < num; ++i ) { info.robotControllerAxisProductCode[i] = py::extract(robotControllerAxisProductCode[i]); } From fa4fd0c4b56dc48bcf41e78ad5a77172f3e2241c Mon Sep 17 00:00:00 2001 From: Leonid Terenin Date: Thu, 7 Nov 2024 10:54:48 +0900 Subject: [PATCH 10/10] change to "smaller-or-equal", take 3 --- python/bindings/openravepy_kinbody.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/python/bindings/openravepy_kinbody.cpp b/python/bindings/openravepy_kinbody.cpp index 3e4ef1b850..1ba16a6e10 100644 --- a/python/bindings/openravepy_kinbody.cpp +++ b/python/bindings/openravepy_kinbody.cpp @@ -1001,7 +1001,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() size_t num1, num2; if( !IS_PYTHONOBJECT_NONE(moveIONames) ) { num1 = len(moveIONames); - OPENRAVE_ASSERT_FORMAT0(num1 == info.moveIONames.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 <= info.moveIONames.size(), _("unexpected size"), ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(moveIONames[py::to_object(i1)])); @@ -1017,7 +1017,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(upperLimitIONames) ) { num1 = len(upperLimitIONames); - OPENRAVE_ASSERT_FORMAT0(num1 == info.upperLimitIONames.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 <= info.upperLimitIONames.size(), _("unexpected size"), ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(upperLimitIONames[py::to_object(i1)])); @@ -1033,7 +1033,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(upperLimitSensorIsOn) ) { num1 = len(upperLimitSensorIsOn); - OPENRAVE_ASSERT_FORMAT0(num1 == info.upperLimitSensorIsOn.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 <= info.upperLimitSensorIsOn.size(), _("unexpected size"), ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(upperLimitSensorIsOn[py::to_object(i1)])); @@ -1049,7 +1049,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(lowerLimitIONames) ) { num1 = len(lowerLimitIONames); - OPENRAVE_ASSERT_FORMAT0(num1 == info.lowerLimitIONames.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 <= info.lowerLimitIONames.size(), _("unexpected size"), ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(lowerLimitIONames[py::to_object(i1)])); @@ -1065,7 +1065,7 @@ JointControlInfo_IOPtr PyJointControlInfo_IO::GetJointControlInfo() if( !IS_PYTHONOBJECT_NONE(lowerLimitSensorIsOn) ) { num1 = len(lowerLimitSensorIsOn); - OPENRAVE_ASSERT_FORMAT0(num1 == info.lowerLimitSensorIsOn.size(), _("unexpected size"), ORE_InvalidState); + OPENRAVE_ASSERT_FORMAT0(num1 <= info.lowerLimitSensorIsOn.size(), _("unexpected size"), ORE_InvalidState); for( size_t i1 = 0; i1 < num1; ++i1 ) { #ifdef USE_PYBIND11_PYTHON_BINDINGS num2 = len(extract(lowerLimitSensorIsOn[py::to_object(i1)]));