From 7cdc0fd6b16cf80694d29cf24416a1e7a9ae208e Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Tue, 17 Jun 2025 17:58:48 +0900 Subject: [PATCH 01/11] Improve the initil guess computation for the acceleration limits for the robot with dynamic limits. - Issue - So far, DynamicsCollisionConstraint::_CheckState checks dynamic limits. - When it's violated, CFO_CheckTimeBasedConstraints is returned and both velocity limits and acceleration limits are reduced. - Even if the dynamic acceleration limits are violated, the velocity is reduced and tends to be slower trajectory. - Observation - In many places to compute the ramp in parabolicsmoother2, boundary conditions are fixed. - In such case, the dynamic acceleration limits as boundary conditions can be computed as necessary condition for the planning. - Resolution - Before each ramp computation, compute the dynamic acceleration limits as necesary condition. --- plugins/rplanners/parabolicsmoother2.cpp | 96 ++++++++++++++++++++++++ 1 file changed, 96 insertions(+) diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp index cf8547dada..6b922faf55 100644 --- a/plugins/rplanners/parabolicsmoother2.cpp +++ b/plugins/rplanners/parabolicsmoother2.cpp @@ -537,6 +537,29 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility OPENRAVE_ASSERT_OP(parameters->_vConfigVelocityLimit.size(), ==, parameters->_vConfigAccelerationLimit.size()); OPENRAVE_ASSERT_OP((int) parameters->_vConfigVelocityLimit.size(), ==, parameters->GetDOF()); + // compute DynamicLimitInfo + _dynamicLimitInfo.Reset(); + FOREACH(itbody, vusedbodies) { + if( !(*itbody) ) { + continue; + } + KinBody& body = *(*itbody); + if( body.IsRobot() ) { + std::vector vUsedConfigIndices; + posSpec.ExtractUsedIndices(KinBodyConstPtr(*itbody), _dynamicLimitInfo.vUsedDOFIndices, vUsedConfigIndices); + if( _dynamicLimitInfo.vUsedDOFIndices.size() == _parameters->_vConfigVelocityLimit.size() ) { + _dynamicLimitInfo.bodyName = body.GetName(); + _dynamicLimitInfo.vFullDOFPositions.resize(body.GetDOF()); + _dynamicLimitInfo.vFullDOFVelocities.assign(body.GetDOF(), 0.0); + _dynamicLimitInfo.vFullDOFAccelerationLimits.resize(body.GetDOF()); + _dynamicLimitInfo.vFullDOFJerkLimits.resize(body.GetDOF()); + _dynamicLimitInfo.bHasDynamicLimits = body.GetDOFDynamicAccelerationJerkLimits(_dynamicLimitInfo.vFullDOFAccelerationLimits, _dynamicLimitInfo.vFullDOFJerkLimits, + _dynamicLimitInfo.vFullDOFPositions, _dynamicLimitInfo.vFullDOFVelocities); + break; + } + } + } + // Retrieve waypoints bool bPathIsPerfectlyModeled = false; // will be true if the initial interpolation is linear or quadratic std::vector q(_parameters->GetDOF()); @@ -1631,6 +1654,15 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility int numTries = 1000; // number of times allowed to scale down vellimits and accellimits RampOptimizer::CheckReturn retseg(0); std::vector _temp(0); + + KinBodyConstPtr usedBody = _dynamicLimitInfo.bHasDynamicLimits ? GetEnv()->GetKinBody(_dynamicLimitInfo.bodyName) : KinBodyConstPtr(); + // update initial guess of acceleration limits based on dynamic limits + if( _dynamicLimitInfo.bHasDynamicLimits && !!usedBody ) { + // this function assumes both boundary velocities are zero. + v0Vect.assign(vellimits.size(), 0.0); + v1Vect.assign(vellimits.size(), 0.0); + _UpdateLimitsByDynamicLimits(accellimits, _parameters->_vConfigVelocityLimit, x0VectIn, x1VectIn, v0Vect, v1Vect, *usedBody); + } for (; itry < numTries; ++itry) { bool res = _interpolator.ComputeZeroVelNDTrajectory(x0VectIn, x1VectIn, vellimits, accellimits, rampndVectOut); if( !res ) { @@ -1813,6 +1845,7 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility size_t index; size_t iters = 0; size_t numIters = _vZeroVelPointInfos.size(); + KinBodyConstPtr usedBody = _dynamicLimitInfo.bHasDynamicLimits ? GetEnv()->GetKinBody(_dynamicLimitInfo.bodyName) : KinBodyConstPtr(); for (index = 0; index < _vZeroVelPointInfos.size(); ++index, ++iters) { // _vZeroVelPointInfos.size() dynamically changes // Sample t0 and t1. We could possibly add some heuristics here to get higher quality // shortcuts @@ -1899,6 +1932,10 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility } } } + // update initial guess of acceleration limits based on dynamic limits + if( _dynamicLimitInfo.bHasDynamicLimits && !!usedBody ) { + _UpdateLimitsByDynamicLimits(accellimits, _parameters->_vConfigVelocityLimit, x0Vect, x1Vect, v0Vect, v1Vect, *usedBody); + } std::vector reductionFactors2; // keeps track of the reduction factors got from this shortcut @@ -2576,6 +2613,7 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility // Main shortcut loop int iters = 0; + KinBodyConstPtr usedBody = _dynamicLimitInfo.bHasDynamicLimits ? GetEnv()->GetKinBody(_dynamicLimitInfo.bodyName) : KinBodyConstPtr(); for (iters = 0; iters < numIters; ++iters) { if( tTotal < minTimeStep ) { #ifdef SMOOTHER2_PROGRESS_DEBUG @@ -2744,6 +2782,10 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility } } } + // update initial guess of acceleration limits based on dynamic limits + if( _dynamicLimitInfo.bHasDynamicLimits && !!usedBody ) { + _UpdateLimitsByDynamicLimits(accellimits, _parameters->_vConfigVelocityLimit, x0Vect, x1Vect, v0Vect, v1Vect, *usedBody); + } std::vector reductionFactors2; // keeps track of the reduction factors got from this shortcut @@ -3591,6 +3633,42 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility _EnsureValidlySampledTimes(t0, t1, tTotal); } + /// \brief update limits by dynamic limits. for now, update only acceleration limits. + /// acceleration limits should at least satisfy the dynamic acceleration limits at t0 and t1, which are start/end boundary conditions of trajectory segment. + /// all vectors in arguments have same size and order config, like _parameters->_vConfigVelocityLimit. + /// \param[out/in] vAccelLimits : resultant acceleration limits. expected to have the initial acceleration limits in it and this function updates them. + /// \param[in] vVelocityLimits : just in case, clamp velocity by velocity limits, since dynamic limit might be ill-condition. + /// \param[in] x0Vect, v0Vect, x1Vect, v1Vect : boundary conditions of positions and velocities at t0 and t1. Same size and order as _parameters->_vConfigVelocityLimit. + /// \param[in] usedBody : used kinbody, which should support GetDOFDynamicAccelerationJerkLimits API. + void _UpdateLimitsByDynamicLimits(std::vector& vAccelLimits, + const std::vector& vVelocityLimits, + const std::vector& x0Vect, const std::vector& x1Vect, + const std::vector& v0Vect, const std::vector& v1Vect, + const KinBody& usedBody) + { + const double fMargin = 0.9999; // margin from the dynamic acceleration limits. even for the case that respecting acceleration limits at t0 and t1 is theoretically enough, there might be numerical error in Check function in DynamicsCollisionConstraint. + // check dynamic acceleration limit at x0 + for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { + _dynamicLimitInfo.vFullDOFPositions[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = x0Vect[iDOF]; + _dynamicLimitInfo.vFullDOFVelocities[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = max(-vVelocityLimits[iDOF], min(v0Vect[iDOF], vVelocityLimits[iDOF])); + } + usedBody.GetDOFDynamicAccelerationJerkLimits(_dynamicLimitInfo.vFullDOFAccelerationLimits, _dynamicLimitInfo.vFullDOFJerkLimits, + _dynamicLimitInfo.vFullDOFPositions, _dynamicLimitInfo.vFullDOFVelocities); + for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { + vAccelLimits[iDOF] = min(_dynamicLimitInfo.vFullDOFAccelerationLimits[_dynamicLimitInfo.vUsedDOFIndices[iDOF]]*fMargin, vAccelLimits[iDOF]); + } + // check dynamic acceleration limit at x1 + for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { + _dynamicLimitInfo.vFullDOFPositions[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = x1Vect[iDOF]; + _dynamicLimitInfo.vFullDOFVelocities[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = max(-vVelocityLimits[iDOF], min(v1Vect[iDOF], vVelocityLimits[iDOF])); + } + usedBody.GetDOFDynamicAccelerationJerkLimits(_dynamicLimitInfo.vFullDOFAccelerationLimits, _dynamicLimitInfo.vFullDOFJerkLimits, + _dynamicLimitInfo.vFullDOFPositions, _dynamicLimitInfo.vFullDOFVelocities); + for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { + vAccelLimits[iDOF] = min(_dynamicLimitInfo.vFullDOFAccelerationLimits[_dynamicLimitInfo.vUsedDOFIndices[iDOF]]*fMargin, vAccelLimits[iDOF]); + } + } + /// Members int _environmentid; ConstraintTrajectoryTimingParametersPtr _parameters; @@ -3671,6 +3749,24 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility std::stringstream _sslog; // for logging purpose + /// \brief info to compute better constraints or heuristics for planning based on dynamic limits. + struct DynamicLimitInfo + { + /// \brief reset + void Reset() + { + bHasDynamicLimits = false; + vUsedDOFIndices.clear(); + bodyName.clear(); + }; + + std::vector vUsedDOFIndices; ///< used openrave dof indices + std::string bodyName; ///< used body name to check dynamic limits in _parameters->_configurationspecification + bool bHasDynamicLimits; ///< true if the body has dynamic limits + std::vector vFullDOFPositions, vFullDOFVelocities, vFullDOFAccelerationLimits, vFullDOFJerkLimits; ///< cached vectors. openrave kinematics order and size is GetDOF. + }; + DynamicLimitInfo _dynamicLimitInfo; + }; // end class ParabolicSmoother2 PlannerBasePtr CreateParabolicSmoother2(EnvironmentBasePtr penv, std::istream& sinput) From d66da8708100731f32bfc1f5a1c5bc23ce79d577 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 18 Jun 2025 14:51:38 +0900 Subject: [PATCH 02/11] Add checking for unsupported dof for dynamic acceleration limits --- plugins/rplanners/parabolicsmoother2.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp index 6b922faf55..8de060b89d 100644 --- a/plugins/rplanners/parabolicsmoother2.cpp +++ b/plugins/rplanners/parabolicsmoother2.cpp @@ -3655,6 +3655,9 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility usedBody.GetDOFDynamicAccelerationJerkLimits(_dynamicLimitInfo.vFullDOFAccelerationLimits, _dynamicLimitInfo.vFullDOFJerkLimits, _dynamicLimitInfo.vFullDOFPositions, _dynamicLimitInfo.vFullDOFVelocities); for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { + if( _dynamicLimitInfo.vFullDOFAccelerationLimits[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] < g_fEpsilon ) { // if dynamic limits are close to zero, this dof does not suppot dynamic limit. so, skip. + continue; + } vAccelLimits[iDOF] = min(_dynamicLimitInfo.vFullDOFAccelerationLimits[_dynamicLimitInfo.vUsedDOFIndices[iDOF]]*fMargin, vAccelLimits[iDOF]); } // check dynamic acceleration limit at x1 @@ -3665,6 +3668,9 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility usedBody.GetDOFDynamicAccelerationJerkLimits(_dynamicLimitInfo.vFullDOFAccelerationLimits, _dynamicLimitInfo.vFullDOFJerkLimits, _dynamicLimitInfo.vFullDOFPositions, _dynamicLimitInfo.vFullDOFVelocities); for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { + if( _dynamicLimitInfo.vFullDOFAccelerationLimits[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] < g_fEpsilon ) { // if dynamic limits are close to zero, this dof does not suppot dynamic limit. so, skip. + continue; + } vAccelLimits[iDOF] = min(_dynamicLimitInfo.vFullDOFAccelerationLimits[_dynamicLimitInfo.vUsedDOFIndices[iDOF]]*fMargin, vAccelLimits[iDOF]); } } From f8eed7bbd4bdf4239626ffbda806f1ea1dcfaf37 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Wed, 18 Jun 2025 14:56:24 +0900 Subject: [PATCH 03/11] Reduce the code duplication --- plugins/rplanners/parabolicsmoother2.cpp | 51 +++++++++++++----------- 1 file changed, 28 insertions(+), 23 deletions(-) diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp index 8de060b89d..b2bb6ade7e 100644 --- a/plugins/rplanners/parabolicsmoother2.cpp +++ b/plugins/rplanners/parabolicsmoother2.cpp @@ -3638,32 +3638,18 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility /// all vectors in arguments have same size and order config, like _parameters->_vConfigVelocityLimit. /// \param[out/in] vAccelLimits : resultant acceleration limits. expected to have the initial acceleration limits in it and this function updates them. /// \param[in] vVelocityLimits : just in case, clamp velocity by velocity limits, since dynamic limit might be ill-condition. - /// \param[in] x0Vect, v0Vect, x1Vect, v1Vect : boundary conditions of positions and velocities at t0 and t1. Same size and order as _parameters->_vConfigVelocityLimit. + /// \param[in] xVect, vVect : boundary conditions of positions and velocities. Same size and order as _parameters->_vConfigVelocityLimit. /// \param[in] usedBody : used kinbody, which should support GetDOFDynamicAccelerationJerkLimits API. - void _UpdateLimitsByDynamicLimits(std::vector& vAccelLimits, - const std::vector& vVelocityLimits, - const std::vector& x0Vect, const std::vector& x1Vect, - const std::vector& v0Vect, const std::vector& v1Vect, - const KinBody& usedBody) + inline void _UpdateLimitsByDynamicLimitsAtBoundary(std::vector& vAccelLimits, + const std::vector& vVelocityLimits, + const std::vector& xVect, const std::vector& vVect, + const KinBody& usedBody) + { - const double fMargin = 0.9999; // margin from the dynamic acceleration limits. even for the case that respecting acceleration limits at t0 and t1 is theoretically enough, there might be numerical error in Check function in DynamicsCollisionConstraint. - // check dynamic acceleration limit at x0 - for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { - _dynamicLimitInfo.vFullDOFPositions[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = x0Vect[iDOF]; - _dynamicLimitInfo.vFullDOFVelocities[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = max(-vVelocityLimits[iDOF], min(v0Vect[iDOF], vVelocityLimits[iDOF])); - } - usedBody.GetDOFDynamicAccelerationJerkLimits(_dynamicLimitInfo.vFullDOFAccelerationLimits, _dynamicLimitInfo.vFullDOFJerkLimits, - _dynamicLimitInfo.vFullDOFPositions, _dynamicLimitInfo.vFullDOFVelocities); - for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { - if( _dynamicLimitInfo.vFullDOFAccelerationLimits[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] < g_fEpsilon ) { // if dynamic limits are close to zero, this dof does not suppot dynamic limit. so, skip. - continue; - } - vAccelLimits[iDOF] = min(_dynamicLimitInfo.vFullDOFAccelerationLimits[_dynamicLimitInfo.vUsedDOFIndices[iDOF]]*fMargin, vAccelLimits[iDOF]); - } - // check dynamic acceleration limit at x1 + constexpr double fMargin = 0.9999; // margin from the dynamic acceleration limits. even for the case that respecting acceleration limits at t0 and t1 is theoretically enough, there might be numerical error in Check function in DynamicsCollisionConstraint. for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { - _dynamicLimitInfo.vFullDOFPositions[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = x1Vect[iDOF]; - _dynamicLimitInfo.vFullDOFVelocities[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = max(-vVelocityLimits[iDOF], min(v1Vect[iDOF], vVelocityLimits[iDOF])); + _dynamicLimitInfo.vFullDOFPositions[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = xVect[iDOF]; + _dynamicLimitInfo.vFullDOFVelocities[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = max(-vVelocityLimits[iDOF], min(vVect[iDOF], vVelocityLimits[iDOF])); } usedBody.GetDOFDynamicAccelerationJerkLimits(_dynamicLimitInfo.vFullDOFAccelerationLimits, _dynamicLimitInfo.vFullDOFJerkLimits, _dynamicLimitInfo.vFullDOFPositions, _dynamicLimitInfo.vFullDOFVelocities); @@ -3675,6 +3661,25 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility } } + /// \brief update limits by dynamic limits. for now, update only acceleration limits. + /// acceleration limits should at least satisfy the dynamic acceleration limits at t0 and t1, which are start/end boundary conditions of trajectory segment. + /// all vectors in arguments have same size and order config, like _parameters->_vConfigVelocityLimit. + /// \param[out/in] vAccelLimits : resultant acceleration limits. expected to have the initial acceleration limits in it and this function updates them. + /// \param[in] vVelocityLimits : just in case, clamp velocity by velocity limits, since dynamic limit might be ill-condition. + /// \param[in] x0Vect, v0Vect, x1Vect, v1Vect : boundary conditions of positions and velocities at t0 and t1. Same size and order as _parameters->_vConfigVelocityLimit. + /// \param[in] usedBody : used kinbody, which should support GetDOFDynamicAccelerationJerkLimits API. + void _UpdateLimitsByDynamicLimits(std::vector& vAccelLimits, + const std::vector& vVelocityLimits, + const std::vector& x0Vect, const std::vector& x1Vect, + const std::vector& v0Vect, const std::vector& v1Vect, + const KinBody& usedBody) + { + // check and update dynamic acceleration limit at x0 + _UpdateLimitsByDynamicLimitsAtBoundary(vAccelLimits, vVelocityLimits, x0Vect, v0Vect, usedBody); + // check and update dynamic acceleration limit at x1 + _UpdateLimitsByDynamicLimitsAtBoundary(vAccelLimits, vVelocityLimits, x1Vect, v1Vect, usedBody); + } + /// Members int _environmentid; ConstraintTrajectoryTimingParametersPtr _parameters; From 7c6ec3a421f876cac16869ca0aa82bd53b8f225c Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 19 Jun 2025 13:26:44 +0900 Subject: [PATCH 04/11] Use shared_ptr of dynamic limit info, not to allocate memory for the robot without dynamic limit. Use shared_ptr of kinbody for computation. --- plugins/rplanners/parabolicsmoother2.cpp | 178 ++++++++++++----------- 1 file changed, 97 insertions(+), 81 deletions(-) diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp index b2bb6ade7e..0beecc5c24 100644 --- a/plugins/rplanners/parabolicsmoother2.cpp +++ b/plugins/rplanners/parabolicsmoother2.cpp @@ -537,24 +537,26 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility OPENRAVE_ASSERT_OP(parameters->_vConfigVelocityLimit.size(), ==, parameters->_vConfigAccelerationLimit.size()); OPENRAVE_ASSERT_OP((int) parameters->_vConfigVelocityLimit.size(), ==, parameters->GetDOF()); - // compute DynamicLimitInfo - _dynamicLimitInfo.Reset(); + // compute DynamicLimitInfo if necessary + _bHasDynamicLimits = false; FOREACH(itbody, vusedbodies) { if( !(*itbody) ) { continue; } KinBody& body = *(*itbody); if( body.IsRobot() ) { - std::vector vUsedConfigIndices; - posSpec.ExtractUsedIndices(KinBodyConstPtr(*itbody), _dynamicLimitInfo.vUsedDOFIndices, vUsedConfigIndices); - if( _dynamicLimitInfo.vUsedDOFIndices.size() == _parameters->_vConfigVelocityLimit.size() ) { - _dynamicLimitInfo.bodyName = body.GetName(); - _dynamicLimitInfo.vFullDOFPositions.resize(body.GetDOF()); - _dynamicLimitInfo.vFullDOFVelocities.assign(body.GetDOF(), 0.0); - _dynamicLimitInfo.vFullDOFAccelerationLimits.resize(body.GetDOF()); - _dynamicLimitInfo.vFullDOFJerkLimits.resize(body.GetDOF()); - _dynamicLimitInfo.bHasDynamicLimits = body.GetDOFDynamicAccelerationJerkLimits(_dynamicLimitInfo.vFullDOFAccelerationLimits, _dynamicLimitInfo.vFullDOFJerkLimits, - _dynamicLimitInfo.vFullDOFPositions, _dynamicLimitInfo.vFullDOFVelocities); + std::vector vUsedDOFIndices, vUsedConfigIndices; + posSpec.ExtractUsedIndices(KinBodyConstPtr(*itbody), vUsedDOFIndices, vUsedConfigIndices); + if( vUsedDOFIndices.size() == _parameters->_vConfigVelocityLimit.size() ) { + std::vector& vFullDOFPositions = _cacheX0Vect, &vFullDOFVelocities = _cacheX1Vect, &vFullDOFAccelerationLimits = _cacheV0Vect, &vFullDOFJerkLimits = _cacheV1Vect; + DynamicLimitInfo::InitializeCachedVectors(vFullDOFPositions, vFullDOFVelocities, vFullDOFAccelerationLimits, vFullDOFJerkLimits, body.GetDOF()); + _bHasDynamicLimits = body.GetDOFDynamicAccelerationJerkLimits(vFullDOFAccelerationLimits, vFullDOFJerkLimits, vFullDOFPositions, vFullDOFVelocities); + if( _bHasDynamicLimits ) { + if( !_pDynamicLimitInfo ) { + _pDynamicLimitInfo.reset(new DynamicLimitInfo()); + } + _pDynamicLimitInfo->Init(*itbody, vUsedDOFIndices); + } break; } } @@ -1655,13 +1657,12 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility RampOptimizer::CheckReturn retseg(0); std::vector _temp(0); - KinBodyConstPtr usedBody = _dynamicLimitInfo.bHasDynamicLimits ? GetEnv()->GetKinBody(_dynamicLimitInfo.bodyName) : KinBodyConstPtr(); // update initial guess of acceleration limits based on dynamic limits - if( _dynamicLimitInfo.bHasDynamicLimits && !!usedBody ) { + if( _bHasDynamicLimits ) { // this function assumes both boundary velocities are zero. v0Vect.assign(vellimits.size(), 0.0); v1Vect.assign(vellimits.size(), 0.0); - _UpdateLimitsByDynamicLimits(accellimits, _parameters->_vConfigVelocityLimit, x0VectIn, x1VectIn, v0Vect, v1Vect, *usedBody); + _pDynamicLimitInfo->UpdateLimitsByDynamicLimits(accellimits, _parameters->_vConfigVelocityLimit, x0VectIn, x1VectIn, v0Vect, v1Vect); } for (; itry < numTries; ++itry) { bool res = _interpolator.ComputeZeroVelNDTrajectory(x0VectIn, x1VectIn, vellimits, accellimits, rampndVectOut); @@ -1845,7 +1846,6 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility size_t index; size_t iters = 0; size_t numIters = _vZeroVelPointInfos.size(); - KinBodyConstPtr usedBody = _dynamicLimitInfo.bHasDynamicLimits ? GetEnv()->GetKinBody(_dynamicLimitInfo.bodyName) : KinBodyConstPtr(); for (index = 0; index < _vZeroVelPointInfos.size(); ++index, ++iters) { // _vZeroVelPointInfos.size() dynamically changes // Sample t0 and t1. We could possibly add some heuristics here to get higher quality // shortcuts @@ -1933,8 +1933,8 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility } } // update initial guess of acceleration limits based on dynamic limits - if( _dynamicLimitInfo.bHasDynamicLimits && !!usedBody ) { - _UpdateLimitsByDynamicLimits(accellimits, _parameters->_vConfigVelocityLimit, x0Vect, x1Vect, v0Vect, v1Vect, *usedBody); + if( _bHasDynamicLimits ) { + _pDynamicLimitInfo->UpdateLimitsByDynamicLimits(accellimits, _parameters->_vConfigVelocityLimit, x0Vect, x1Vect, v0Vect, v1Vect); } std::vector reductionFactors2; // keeps track of the reduction factors got from this shortcut @@ -2613,7 +2613,6 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility // Main shortcut loop int iters = 0; - KinBodyConstPtr usedBody = _dynamicLimitInfo.bHasDynamicLimits ? GetEnv()->GetKinBody(_dynamicLimitInfo.bodyName) : KinBodyConstPtr(); for (iters = 0; iters < numIters; ++iters) { if( tTotal < minTimeStep ) { #ifdef SMOOTHER2_PROGRESS_DEBUG @@ -2783,8 +2782,8 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility } } // update initial guess of acceleration limits based on dynamic limits - if( _dynamicLimitInfo.bHasDynamicLimits && !!usedBody ) { - _UpdateLimitsByDynamicLimits(accellimits, _parameters->_vConfigVelocityLimit, x0Vect, x1Vect, v0Vect, v1Vect, *usedBody); + if( _bHasDynamicLimits ) { + _pDynamicLimitInfo->UpdateLimitsByDynamicLimits(accellimits, _parameters->_vConfigVelocityLimit, x0Vect, x1Vect, v0Vect, v1Vect); } std::vector reductionFactors2; // keeps track of the reduction factors got from this shortcut @@ -3633,53 +3632,6 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility _EnsureValidlySampledTimes(t0, t1, tTotal); } - /// \brief update limits by dynamic limits. for now, update only acceleration limits. - /// acceleration limits should at least satisfy the dynamic acceleration limits at t0 and t1, which are start/end boundary conditions of trajectory segment. - /// all vectors in arguments have same size and order config, like _parameters->_vConfigVelocityLimit. - /// \param[out/in] vAccelLimits : resultant acceleration limits. expected to have the initial acceleration limits in it and this function updates them. - /// \param[in] vVelocityLimits : just in case, clamp velocity by velocity limits, since dynamic limit might be ill-condition. - /// \param[in] xVect, vVect : boundary conditions of positions and velocities. Same size and order as _parameters->_vConfigVelocityLimit. - /// \param[in] usedBody : used kinbody, which should support GetDOFDynamicAccelerationJerkLimits API. - inline void _UpdateLimitsByDynamicLimitsAtBoundary(std::vector& vAccelLimits, - const std::vector& vVelocityLimits, - const std::vector& xVect, const std::vector& vVect, - const KinBody& usedBody) - - { - constexpr double fMargin = 0.9999; // margin from the dynamic acceleration limits. even for the case that respecting acceleration limits at t0 and t1 is theoretically enough, there might be numerical error in Check function in DynamicsCollisionConstraint. - for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { - _dynamicLimitInfo.vFullDOFPositions[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = xVect[iDOF]; - _dynamicLimitInfo.vFullDOFVelocities[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] = max(-vVelocityLimits[iDOF], min(vVect[iDOF], vVelocityLimits[iDOF])); - } - usedBody.GetDOFDynamicAccelerationJerkLimits(_dynamicLimitInfo.vFullDOFAccelerationLimits, _dynamicLimitInfo.vFullDOFJerkLimits, - _dynamicLimitInfo.vFullDOFPositions, _dynamicLimitInfo.vFullDOFVelocities); - for(int iDOF = 0; iDOF < (int)_dynamicLimitInfo.vUsedDOFIndices.size(); ++iDOF) { - if( _dynamicLimitInfo.vFullDOFAccelerationLimits[_dynamicLimitInfo.vUsedDOFIndices[iDOF]] < g_fEpsilon ) { // if dynamic limits are close to zero, this dof does not suppot dynamic limit. so, skip. - continue; - } - vAccelLimits[iDOF] = min(_dynamicLimitInfo.vFullDOFAccelerationLimits[_dynamicLimitInfo.vUsedDOFIndices[iDOF]]*fMargin, vAccelLimits[iDOF]); - } - } - - /// \brief update limits by dynamic limits. for now, update only acceleration limits. - /// acceleration limits should at least satisfy the dynamic acceleration limits at t0 and t1, which are start/end boundary conditions of trajectory segment. - /// all vectors in arguments have same size and order config, like _parameters->_vConfigVelocityLimit. - /// \param[out/in] vAccelLimits : resultant acceleration limits. expected to have the initial acceleration limits in it and this function updates them. - /// \param[in] vVelocityLimits : just in case, clamp velocity by velocity limits, since dynamic limit might be ill-condition. - /// \param[in] x0Vect, v0Vect, x1Vect, v1Vect : boundary conditions of positions and velocities at t0 and t1. Same size and order as _parameters->_vConfigVelocityLimit. - /// \param[in] usedBody : used kinbody, which should support GetDOFDynamicAccelerationJerkLimits API. - void _UpdateLimitsByDynamicLimits(std::vector& vAccelLimits, - const std::vector& vVelocityLimits, - const std::vector& x0Vect, const std::vector& x1Vect, - const std::vector& v0Vect, const std::vector& v1Vect, - const KinBody& usedBody) - { - // check and update dynamic acceleration limit at x0 - _UpdateLimitsByDynamicLimitsAtBoundary(vAccelLimits, vVelocityLimits, x0Vect, v0Vect, usedBody); - // check and update dynamic acceleration limit at x1 - _UpdateLimitsByDynamicLimitsAtBoundary(vAccelLimits, vVelocityLimits, x1Vect, v1Vect, usedBody); - } - /// Members int _environmentid; ConstraintTrajectoryTimingParametersPtr _parameters; @@ -3761,22 +3713,86 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility std::stringstream _sslog; // for logging purpose /// \brief info to compute better constraints or heuristics for planning based on dynamic limits. - struct DynamicLimitInfo + class DynamicLimitInfo { - /// \brief reset - void Reset() + public: + /// \brief initialize dynamic limit info. + /// \param[in] pUsedBody : used kinbody. + /// \param[in] vUsedDOFIndices : used dof indices for config. + void Init(const OpenRAVE::KinBodyConstPtr& pUsedBody, const std::vector& vUsedDOFIndices) + { + _pUsedBody = pUsedBody; + _vUsedDOFIndices = vUsedDOFIndices; + InitializeCachedVectors(_vFullDOFPositions, _vFullDOFVelocities, _vFullDOFAccelerationLimits, _vFullDOFJerkLimits, _pUsedBody->GetDOF()); + } + + /// \brief update limits by dynamic limits. for now, update only acceleration limits. + /// acceleration limits should at least satisfy the dynamic acceleration limits at t0 and t1, which are start/end boundary conditions of trajectory segment. + /// all vectors in arguments have same size and order config, like _parameters->_vConfigVelocityLimit. + /// \param[out/in] vAccelLimits : resultant acceleration limits. expected to have the initial acceleration limits in it and this function updates them. + /// \param[in] vVelocityLimits : just in case, clamp velocity by velocity limits, since dynamic limit might be ill-condition. + /// \param[in] x0Vect, v0Vect, x1Vect, v1Vect : boundary conditions of positions and velocities at t0 and t1. Same size and order as _parameters->_vConfigVelocityLimit. + /// \param[in] usedBody : used kinbody, which should support GetDOFDynamicAccelerationJerkLimits API. + void UpdateLimitsByDynamicLimits(std::vector& vAccelLimits, + const std::vector& vVelocityLimits, + const std::vector& x0Vect, const std::vector& x1Vect, + const std::vector& v0Vect, const std::vector& v1Vect) + { + const OpenRAVE::KinBody& usedBody = *_pUsedBody; + // check and update dynamic acceleration limit at x0 + _UpdateLimitsByDynamicLimitsAtBoundary(vAccelLimits, vVelocityLimits, x0Vect, v0Vect, usedBody); + // check and update dynamic acceleration limit at x1 + _UpdateLimitsByDynamicLimitsAtBoundary(vAccelLimits, vVelocityLimits, x1Vect, v1Vect, usedBody); + } + + /// \brief initialize cached vector. + static void InitializeCachedVectors(std::vector& vFullDOFPositions, + std::vector& vFullDOFVelocities, + std::vector& vFullDOFAccelerationLimits, + std::vector& vFullDOFJerkLimits, + const int nDOF) + { + vFullDOFPositions.assign(nDOF, 0.0); + vFullDOFVelocities.assign(nDOF, 0.0); + vFullDOFAccelerationLimits.resize(nDOF); + vFullDOFJerkLimits.resize(nDOF); + } + + protected: + /// \brief update limits by dynamic limits. for now, update only acceleration limits. + /// acceleration limits should at least satisfy the dynamic acceleration limits at t0 and t1, which are start/end boundary conditions of trajectory segment. + /// all vectors in arguments have same size and order config, like _parameters->_vConfigVelocityLimit. + /// \param[out/in] vAccelLimits : resultant acceleration limits. expected to have the initial acceleration limits in it and this function updates them. + /// \param[in] vVelocityLimits : just in case, clamp velocity by velocity limits, since dynamic limit might be ill-condition. + /// \param[in] xVect, vVect : boundary conditions of positions and velocities. Same size and order as _parameters->_vConfigVelocityLimit. + /// \param[in] usedBody : used kinbody, which should support GetDOFDynamicAccelerationJerkLimits API. + inline void _UpdateLimitsByDynamicLimitsAtBoundary(std::vector& vAccelLimits, + const std::vector& vVelocityLimits, + const std::vector& xVect, const std::vector& vVect, + const KinBody& usedBody) + { - bHasDynamicLimits = false; - vUsedDOFIndices.clear(); - bodyName.clear(); - }; - - std::vector vUsedDOFIndices; ///< used openrave dof indices - std::string bodyName; ///< used body name to check dynamic limits in _parameters->_configurationspecification - bool bHasDynamicLimits; ///< true if the body has dynamic limits - std::vector vFullDOFPositions, vFullDOFVelocities, vFullDOFAccelerationLimits, vFullDOFJerkLimits; ///< cached vectors. openrave kinematics order and size is GetDOF. + constexpr double fMargin = 0.9999; // margin from the dynamic acceleration limits. even for the case that respecting acceleration limits at t0 and t1 is theoretically enough, there might be numerical error in Check function in DynamicsCollisionConstraint. + for(int iDOF = 0; iDOF < (int)_vUsedDOFIndices.size(); ++iDOF) { + _vFullDOFPositions[_vUsedDOFIndices[iDOF]] = xVect[iDOF]; + _vFullDOFVelocities[_vUsedDOFIndices[iDOF]] = max(-vVelocityLimits[iDOF], min(vVect[iDOF], vVelocityLimits[iDOF])); + } + usedBody.GetDOFDynamicAccelerationJerkLimits(_vFullDOFAccelerationLimits, _vFullDOFJerkLimits, + _vFullDOFPositions, _vFullDOFVelocities); + for(int iDOF = 0; iDOF < (int)_vUsedDOFIndices.size(); ++iDOF) { + if( _vFullDOFAccelerationLimits[_vUsedDOFIndices[iDOF]] < g_fEpsilon ) { // if dynamic limits are close to zero, this dof does not suppot dynamic limit. so, skip. + continue; + } + vAccelLimits[iDOF] = min(_vFullDOFAccelerationLimits[_vUsedDOFIndices[iDOF]]*fMargin, vAccelLimits[iDOF]); + } + } + + std::vector _vUsedDOFIndices; ///< used openrave dof indices + std::vector _vFullDOFPositions, _vFullDOFVelocities, _vFullDOFAccelerationLimits, _vFullDOFJerkLimits; ///< cached vectors. openrave kinematics order and size is GetDOF. + OpenRAVE::KinBodyConstPtr _pUsedBody; ///< used kinbody. }; - DynamicLimitInfo _dynamicLimitInfo; + boost::shared_ptr _pDynamicLimitInfo; ///< ptr of info for dynamic limit computation. + bool _bHasDynamicLimits = false; ///< true if the used kinbody has dynamic limits and this parabolicsmoother2 needs to compute it. }; // end class ParabolicSmoother2 From 94a64cd348b1c0672e4c019ab8c0fc796741be73 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 19 Jun 2025 17:22:17 +0900 Subject: [PATCH 05/11] simplify if-else logic. --- plugins/rplanners/parabolicsmoother2.cpp | 30 +++++++++++------------- 1 file changed, 14 insertions(+), 16 deletions(-) diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp index 0beecc5c24..cae37e7353 100644 --- a/plugins/rplanners/parabolicsmoother2.cpp +++ b/plugins/rplanners/parabolicsmoother2.cpp @@ -540,26 +540,24 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility // compute DynamicLimitInfo if necessary _bHasDynamicLimits = false; FOREACH(itbody, vusedbodies) { - if( !(*itbody) ) { + if( !(*itbody) || !(*itbody)->IsRobot() ) { continue; } - KinBody& body = *(*itbody); - if( body.IsRobot() ) { - std::vector vUsedDOFIndices, vUsedConfigIndices; - posSpec.ExtractUsedIndices(KinBodyConstPtr(*itbody), vUsedDOFIndices, vUsedConfigIndices); - if( vUsedDOFIndices.size() == _parameters->_vConfigVelocityLimit.size() ) { - std::vector& vFullDOFPositions = _cacheX0Vect, &vFullDOFVelocities = _cacheX1Vect, &vFullDOFAccelerationLimits = _cacheV0Vect, &vFullDOFJerkLimits = _cacheV1Vect; - DynamicLimitInfo::InitializeCachedVectors(vFullDOFPositions, vFullDOFVelocities, vFullDOFAccelerationLimits, vFullDOFJerkLimits, body.GetDOF()); - _bHasDynamicLimits = body.GetDOFDynamicAccelerationJerkLimits(vFullDOFAccelerationLimits, vFullDOFJerkLimits, vFullDOFPositions, vFullDOFVelocities); - if( _bHasDynamicLimits ) { - if( !_pDynamicLimitInfo ) { - _pDynamicLimitInfo.reset(new DynamicLimitInfo()); - } - _pDynamicLimitInfo->Init(*itbody, vUsedDOFIndices); - } - break; + std::vector vUsedDOFIndices, vUsedConfigIndices; + posSpec.ExtractUsedIndices(KinBodyConstPtr(*itbody), vUsedDOFIndices, vUsedConfigIndices); + if( vUsedDOFIndices.size() != _parameters->_vConfigVelocityLimit.size() ) { + continue; + } + std::vector& vFullDOFPositions = _cacheX0Vect, &vFullDOFVelocities = _cacheX1Vect, &vFullDOFAccelerationLimits = _cacheV0Vect, &vFullDOFJerkLimits = _cacheV1Vect; + DynamicLimitInfo::InitializeCachedVectors(vFullDOFPositions, vFullDOFVelocities, vFullDOFAccelerationLimits, vFullDOFJerkLimits, (*itbody)->GetDOF()); + _bHasDynamicLimits = (*itbody)->GetDOFDynamicAccelerationJerkLimits(vFullDOFAccelerationLimits, vFullDOFJerkLimits, vFullDOFPositions, vFullDOFVelocities); + if( _bHasDynamicLimits ) { + if( !_pDynamicLimitInfo ) { + _pDynamicLimitInfo.reset(new DynamicLimitInfo()); } + _pDynamicLimitInfo->Init(*itbody, vUsedDOFIndices); } + break; } // Retrieve waypoints From 56938705cf0d4854f1b332c118cc328e9da4964f Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 26 Jun 2025 10:45:11 +0900 Subject: [PATCH 06/11] Update plugins/rplanners/parabolicsmoother2.cpp Instead of forcibly setting the dof values to zeros, get the correct current values. Co-authored-by: Puttichai Lertkultanon --- plugins/rplanners/parabolicsmoother2.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp index cae37e7353..3d7a34bd7c 100644 --- a/plugins/rplanners/parabolicsmoother2.cpp +++ b/plugins/rplanners/parabolicsmoother2.cpp @@ -3743,17 +3743,18 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility _UpdateLimitsByDynamicLimitsAtBoundary(vAccelLimits, vVelocityLimits, x1Vect, v1Vect, usedBody); } - /// \brief initialize cached vector. - static void InitializeCachedVectors(std::vector& vFullDOFPositions, + /// \brief initialize cached vectors. + static void InitializeCachedVectors(const OpenRAVE::KinBodyConstPtr& pBody, + std::vector& vFullDOFPositions, std::vector& vFullDOFVelocities, std::vector& vFullDOFAccelerationLimits, - std::vector& vFullDOFJerkLimits, - const int nDOF) + std::vector& vFullDOFJerkLimits) { - vFullDOFPositions.assign(nDOF, 0.0); - vFullDOFVelocities.assign(nDOF, 0.0); - vFullDOFAccelerationLimits.resize(nDOF); - vFullDOFJerkLimits.resize(nDOF); + pBody->GetDOFValues(vFullDOFPositions); + const int nFullDOF = pBody->GetDOF(); + vFullDOFVelocities.assign(nFullDOF, 0.0); + vFullDOFAccelerationLimits.resize(nFullDOF); + vFullDOFJerkLimits.resize(nFullDOF); } protected: From ef86515ff411e3c4d3c1266defb847edd088eb4c Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 26 Jun 2025 10:45:56 +0900 Subject: [PATCH 07/11] Update plugins/rplanners/parabolicsmoother2.cpp Clarify the meaning of dynamic limits. Co-authored-by: Puttichai Lertkultanon --- plugins/rplanners/parabolicsmoother2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp index 3d7a34bd7c..61387e1d40 100644 --- a/plugins/rplanners/parabolicsmoother2.cpp +++ b/plugins/rplanners/parabolicsmoother2.cpp @@ -3791,7 +3791,7 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility OpenRAVE::KinBodyConstPtr _pUsedBody; ///< used kinbody. }; boost::shared_ptr _pDynamicLimitInfo; ///< ptr of info for dynamic limit computation. - bool _bHasDynamicLimits = false; ///< true if the used kinbody has dynamic limits and this parabolicsmoother2 needs to compute it. + bool _bHasDynamicLimits = false; ///< true if the used kinbody has dynamic limits (accel/jerk limits that change based on the current dof values and velocities) and this parabolicsmoother2 needs to compute it. }; // end class ParabolicSmoother2 From f1f11b26bbf8348cf4d1c877724959acd249a9a8 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 26 Jun 2025 10:46:46 +0900 Subject: [PATCH 08/11] Update plugins/rplanners/parabolicsmoother2.cpp Make the comment more precise. Co-authored-by: Puttichai Lertkultanon --- plugins/rplanners/parabolicsmoother2.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp index 61387e1d40..9990719770 100644 --- a/plugins/rplanners/parabolicsmoother2.cpp +++ b/plugins/rplanners/parabolicsmoother2.cpp @@ -3779,7 +3779,7 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility usedBody.GetDOFDynamicAccelerationJerkLimits(_vFullDOFAccelerationLimits, _vFullDOFJerkLimits, _vFullDOFPositions, _vFullDOFVelocities); for(int iDOF = 0; iDOF < (int)_vUsedDOFIndices.size(); ++iDOF) { - if( _vFullDOFAccelerationLimits[_vUsedDOFIndices[iDOF]] < g_fEpsilon ) { // if dynamic limits are close to zero, this dof does not suppot dynamic limit. so, skip. + if( _vFullDOFAccelerationLimits[_vUsedDOFIndices[iDOF]] < g_fEpsilon ) { // if dynamic limits are zero, this dof does not suppot dynamic limit. so skipping. continue; } vAccelLimits[iDOF] = min(_vFullDOFAccelerationLimits[_vUsedDOFIndices[iDOF]]*fMargin, vAccelLimits[iDOF]); From 6b327c8ea07ecaa58ebf4fb9d52b9a1dc0019115 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 26 Jun 2025 10:50:58 +0900 Subject: [PATCH 09/11] Bump patch version to improve parabolicsmoother2 computation for robots with dynamic limits. --- CMakeLists.txt | 2 +- docs/source/changelog.rst | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ab01ff1df7..51a96fb7c4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE ) # Define here the needed parameters set (OPENRAVE_VERSION_MAJOR 0) set (OPENRAVE_VERSION_MINOR 167) -set (OPENRAVE_VERSION_PATCH 5) +set (OPENRAVE_VERSION_PATCH 6) set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH}) set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}) message(STATUS "Compiling OpenRAVE Version ${OPENRAVE_VERSION}, soversion=${OPENRAVE_SOVERSION}") diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst index 75b743ad5c..c537e435d9 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -3,6 +3,11 @@ ChangeLog ######### +Version 0.167.6 +=============== + +- Improve `parabolicsmoother2` computation for the robot with dynamic limits by considering the better initial guess of acceleration limits based on the dynamic acceleration limits at boundaries. + Version 0.167.5 =============== From 0b435e264325f4c194ba6bf9dfe7ad9db4a9672f Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Sat, 28 Jun 2025 15:16:23 +0900 Subject: [PATCH 10/11] Fix compile --- plugins/rplanners/parabolicsmoother2.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp index 9990719770..da1edc3ecb 100644 --- a/plugins/rplanners/parabolicsmoother2.cpp +++ b/plugins/rplanners/parabolicsmoother2.cpp @@ -549,7 +549,7 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility continue; } std::vector& vFullDOFPositions = _cacheX0Vect, &vFullDOFVelocities = _cacheX1Vect, &vFullDOFAccelerationLimits = _cacheV0Vect, &vFullDOFJerkLimits = _cacheV1Vect; - DynamicLimitInfo::InitializeCachedVectors(vFullDOFPositions, vFullDOFVelocities, vFullDOFAccelerationLimits, vFullDOFJerkLimits, (*itbody)->GetDOF()); + DynamicLimitInfo::InitializeCachedVectors((*itbody), vFullDOFPositions, vFullDOFVelocities, vFullDOFAccelerationLimits, vFullDOFJerkLimits); _bHasDynamicLimits = (*itbody)->GetDOFDynamicAccelerationJerkLimits(vFullDOFAccelerationLimits, vFullDOFJerkLimits, vFullDOFPositions, vFullDOFVelocities); if( _bHasDynamicLimits ) { if( !_pDynamicLimitInfo ) { @@ -3721,7 +3721,7 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility { _pUsedBody = pUsedBody; _vUsedDOFIndices = vUsedDOFIndices; - InitializeCachedVectors(_vFullDOFPositions, _vFullDOFVelocities, _vFullDOFAccelerationLimits, _vFullDOFJerkLimits, _pUsedBody->GetDOF()); + InitializeCachedVectors(_pUsedBody, _vFullDOFPositions, _vFullDOFVelocities, _vFullDOFAccelerationLimits, _vFullDOFJerkLimits); } /// \brief update limits by dynamic limits. for now, update only acceleration limits. From 5b18be4e5bf96d5423c57559ea1da024faf7f7fa Mon Sep 17 00:00:00 2001 From: "rosen.diankov@gmail.com" Date: Mon, 21 Jul 2025 14:16:15 -0400 Subject: [PATCH 11/11] use dReal and epsilon correctly --- plugins/rplanners/parabolicsmoother2.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/plugins/rplanners/parabolicsmoother2.cpp b/plugins/rplanners/parabolicsmoother2.cpp index da1edc3ecb..ef1fa67b81 100644 --- a/plugins/rplanners/parabolicsmoother2.cpp +++ b/plugins/rplanners/parabolicsmoother2.cpp @@ -32,9 +32,11 @@ namespace rplanners { namespace RampOptimizer = RampOptimizerInternal; -class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::FeasibilityCheckerBase, public RampOptimizer::RandomNumberGeneratorBase { +class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::FeasibilityCheckerBase, public RampOptimizer::RandomNumberGeneratorBase +{ - class MyRampNDFeasibilityChecker : public RampOptimizer::RampNDFeasibilityChecker { + class MyRampNDFeasibilityChecker : public RampOptimizer::RampNDFeasibilityChecker + { public: MyRampNDFeasibilityChecker(RampOptimizer::FeasibilityCheckerBase* feas_) : RampOptimizer::RampNDFeasibilityChecker(feas_) { _bHasParameters = false; @@ -3713,7 +3715,7 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility /// \brief info to compute better constraints or heuristics for planning based on dynamic limits. class DynamicLimitInfo { - public: +public: /// \brief initialize dynamic limit info. /// \param[in] pUsedBody : used kinbody. /// \param[in] vUsedDOFIndices : used dof indices for config. @@ -3757,7 +3759,7 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility vFullDOFJerkLimits.resize(nFullDOF); } - protected: +protected: /// \brief update limits by dynamic limits. for now, update only acceleration limits. /// acceleration limits should at least satisfy the dynamic acceleration limits at t0 and t1, which are start/end boundary conditions of trajectory segment. /// all vectors in arguments have same size and order config, like _parameters->_vConfigVelocityLimit. @@ -3771,7 +3773,7 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility const KinBody& usedBody) { - constexpr double fMargin = 0.9999; // margin from the dynamic acceleration limits. even for the case that respecting acceleration limits at t0 and t1 is theoretically enough, there might be numerical error in Check function in DynamicsCollisionConstraint. + constexpr dReal fMargin = 0.9999; // margin from the dynamic acceleration limits. even for the case that respecting acceleration limits at t0 and t1 is theoretically enough, there might be numerical error in Check function in DynamicsCollisionConstraint. for(int iDOF = 0; iDOF < (int)_vUsedDOFIndices.size(); ++iDOF) { _vFullDOFPositions[_vUsedDOFIndices[iDOF]] = xVect[iDOF]; _vFullDOFVelocities[_vUsedDOFIndices[iDOF]] = max(-vVelocityLimits[iDOF], min(vVect[iDOF], vVelocityLimits[iDOF])); @@ -3779,7 +3781,7 @@ class ParabolicSmoother2 : public PlannerBase, public RampOptimizer::Feasibility usedBody.GetDOFDynamicAccelerationJerkLimits(_vFullDOFAccelerationLimits, _vFullDOFJerkLimits, _vFullDOFPositions, _vFullDOFVelocities); for(int iDOF = 0; iDOF < (int)_vUsedDOFIndices.size(); ++iDOF) { - if( _vFullDOFAccelerationLimits[_vUsedDOFIndices[iDOF]] < g_fEpsilon ) { // if dynamic limits are zero, this dof does not suppot dynamic limit. so skipping. + if( _vFullDOFAccelerationLimits[_vUsedDOFIndices[iDOF]] <= g_fEpsilon ) { // if dynamic limits are zero, this dof does not suppot dynamic limit. so skipping. continue; } vAccelLimits[iDOF] = min(_vFullDOFAccelerationLimits[_vUsedDOFIndices[iDOF]]*fMargin, vAccelLimits[iDOF]);