Skip to content

Commit

Permalink
Merge branch 'production' into dev/lenik/add_virtual_destructors
Browse files Browse the repository at this point in the history
  • Loading branch information
Leonid Terenin committed Nov 8, 2024
2 parents ff764ec + 1bdb8ad commit 6d74f1f
Show file tree
Hide file tree
Showing 3 changed files with 56 additions and 56 deletions.
48 changes: 24 additions & 24 deletions python/bindings/openravepy_kinbody.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<int>(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<dReal>(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<dReal>(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<std::string>(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<std::string>(robotControllerAxisProductCode[i]);
}
Expand Down Expand Up @@ -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<py::object>(moveIONames[py::to_object(i1)]));
Expand All @@ -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<py::object>(upperLimitIONames[py::to_object(i1)]));
Expand All @@ -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<py::object>(upperLimitSensorIsOn[py::to_object(i1)]));
Expand All @@ -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<py::object>(lowerLimitIONames[py::to_object(i1)]));
Expand All @@ -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<py::object>(lowerLimitSensorIsOn[py::to_object(i1)]));
Expand Down Expand Up @@ -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)]);
}
Expand All @@ -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<dReal>(_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<dReal>(_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<dReal>(_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<dReal>(_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<dReal>(_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<dReal>(_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<dReal>(_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<dReal>(_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<dReal>(_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<dReal>(_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<dReal>(_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<dReal>(_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<dReal>(_vupperlimit[py::to_object(i)]);
}
Expand Down
10 changes: 5 additions & 5 deletions sandbox/parabolicsmoother/interpolation.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]))
Expand All @@ -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)
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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()
Expand Down
Loading

0 comments on commit 6d74f1f

Please sign in to comment.