Created
February 17, 2021 03:25
-
-
Save ompugao/4efff6b50e0c585e43312d2a85b48268 to your computer and use it in GitHub Desktop.
openrave_ubuntu20.04_diff_2.patch
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
diff --git a/.gitignore b/.gitignore | |
index d6136dbab..956bb7caf 100644 | |
--- a/.gitignore | |
+++ b/.gitignore | |
@@ -8,3 +8,4 @@ msvc_soqt/ | |
src/models/ | |
myrelpath.pyc | |
+build | |
diff --git a/CMakeLists.txt b/CMakeLists.txt | |
index 4b8575f52..e0863e228 100644 | |
--- a/CMakeLists.txt | |
+++ b/CMakeLists.txt | |
@@ -630,7 +630,7 @@ if( OPT_COLLADA ) | |
set(COLLADA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/msvc_collada) | |
endif() | |
#find_package(COLLADA_DOM 2.4 COMPONENTS 1.5 1.4 PATHS ${COLLADA_PATH}) | |
- find_package(COLLADA_DOM 2.3 COMPONENTS 1.5 PATHS ${COLLADA_PATH}) | |
+ find_package(COLLADA_DOM 2.5 COMPONENTS 1.5 PATHS ${COLLADA_PATH}) | |
if( COLLADA_DOM_FOUND ) | |
if( COLLADA_DOM_VERSION VERSION_LESS "2.5.0" ) | |
diff --git a/include/openrave/planner.h b/include/openrave/planner.h | |
index 023bb0844..d1e75688c 100644 | |
--- a/include/openrave/planner.h | |
+++ b/include/openrave/planner.h | |
@@ -268,7 +268,8 @@ private: | |
inline int CheckPathAllConstraints(const std::vector<dReal>& q0, const std::vector<dReal>& q1, const std::vector<dReal>& dq0, const std::vector<dReal>& dq1, dReal elapsedtime, IntervalType interval, int options=0xffff, ConstraintFilterReturnPtr filterreturn=ConstraintFilterReturnPtr()) const | |
{ | |
if( !_checkpathvelocityconstraintsfn ) { | |
- return true; | |
+ //return true; | |
+ return 0; | |
} | |
return _checkpathvelocityconstraintsfn(q0, q1, dq0, dq1, elapsedtime, interval, options, filterreturn); | |
} | |
diff --git a/plugins/bulletrave/CMakeLists.txt b/plugins/bulletrave/CMakeLists.txt | |
index 4d7a82300..34c5e3f5d 100644 | |
--- a/plugins/bulletrave/CMakeLists.txt | |
+++ b/plugins/bulletrave/CMakeLists.txt | |
@@ -8,12 +8,12 @@ if( OPT_BULLET ) | |
if( "${bullet_VERSION}" VERSION_GREATER "2.82" ) | |
message(STATUS "unsupported bullet version ${bullet_VERSION}") | |
else() | |
- include_directories(${bullet_INCLUDE_DIRS}) | |
+ include_directories(/usr/local/${bullet_INCLUDE_DIRS}) | |
link_directories(${OPENRAVE_LINK_DIRS} ${bullet_LIBRARY_DIRS}) | |
add_library(bulletrave SHARED bulletrave.cpp bulletcollision.h bulletphysics.h bulletspace.h plugindefs.h) | |
- #message(STATUS "Bullet found ${bullet_INCLUDE_DIRS}, libs: ${bullet_LIBRARIES}, cflags=${bullet_CFLAGS_OTHER}, lflags=${bullet_LDFLAGS_OTHER}, building bulletrave plugin") | |
+ message(STATUS "Bullet found ${bullet_INCLUDE_DIRS}, libs: ${bullet_LIBRARIES}, cflags=${bullet_CFLAGS_OTHER}, lflags=${bullet_LDFLAGS_OTHER}, building bulletrave plugin") | |
target_link_libraries(bulletrave libopenrave ${bullet_LIBRARIES}) | |
target_link_libraries(bulletrave PRIVATE boost_assertion_failed) | |
diff --git a/plugins/bulletrave/bulletspace.h b/plugins/bulletrave/bulletspace.h | |
index e8aad1b51..0f208b84c 100644 | |
--- a/plugins/bulletrave/bulletspace.h | |
+++ b/plugins/bulletrave/bulletspace.h | |
@@ -440,7 +440,7 @@ private: | |
void _Synchronize(KinBodyInfoPtr pinfo) | |
{ | |
vector<Transform> vtrans; | |
- std::vector<int> dofbranches; | |
+ std::vector<dReal> dofbranches; | |
pinfo->pbody->GetLinkTransformations(vtrans,dofbranches); | |
pinfo->nLastStamp = pinfo->pbody->GetUpdateStamp(); | |
BOOST_ASSERT( vtrans.size() == pinfo->vlinks.size() ); | |
diff --git a/plugins/fclrave/fclcollision.h b/plugins/fclrave/fclcollision.h | |
index 6a8e36d28..31fc5dde4 100644 | |
--- a/plugins/fclrave/fclcollision.h | |
+++ b/plugins/fclrave/fclcollision.h | |
@@ -130,6 +130,8 @@ public: | |
// TODO : Consider removing these which could be more harmful than anything else | |
RegisterCommand("SetBroadphaseAlgorithm", boost::bind(&FCLCollisionChecker::SetBroadphaseAlgorithmCommand, this, _1, _2), "sets the broadphase algorithm (Naive, SaP, SSaP, IntervalTree, DynamicAABBTree, DynamicAABBTree_Array)"); | |
RegisterCommand("SetBVHRepresentation", boost::bind(&FCLCollisionChecker::_SetBVHRepresentation, this, _1, _2), "sets the Bouding Volume Hierarchy representation for meshes (AABB, OBB, OBBRSS, RSS, kIDS)"); | |
+ RegisterCommand("ResetStatistics", boost::bind(&FCLCollisionChecker::_ResetStatistics, this, _1, _2), ""); | |
+ RegisterCommand("DisplayStatistics", boost::bind(&FCLCollisionChecker::_DisplayStatistics, this, _1, _2), ""); | |
RAVELOG_VERBOSE_FORMAT("FCLCollisionChecker %s created in env %d", _userdatakey%penv->GetId()); | |
@@ -893,6 +895,19 @@ public: | |
private: | |
+ bool _ResetStatistics(ostream& sout, istream& sinput) | |
+ { | |
+ RESET_TIMINGS(_statistics); | |
+ return !!sinput; | |
+ } | |
+ bool _DisplayStatistics(ostream& sout, istream& sinput) | |
+ { | |
+ std::string fileprefix; | |
+ sinput >> fileprefix; | |
+ DISPLAY(_statistics, fileprefix); | |
+ return !!sinput; | |
+ } | |
+ | |
inline boost::shared_ptr<FCLCollisionChecker> shared_checker() { | |
return boost::static_pointer_cast<FCLCollisionChecker>(shared_from_this()); | |
} | |
diff --git a/plugins/fclrave/fclstatistics.h b/plugins/fclrave/fclstatistics.h | |
index a4c9f5cad..d7a178066 100644 | |
--- a/plugins/fclrave/fclstatistics.h | |
+++ b/plugins/fclrave/fclstatistics.h | |
@@ -53,9 +53,9 @@ public: | |
RAVELOG_WARN_FORMAT("FCL STATISTICS;%s%s", label % ss.str()); | |
} | |
- void Display() { | |
+ void Display(const std::string& fileprefix="") { | |
EnvironmentMutex::scoped_lock lock(log_out_mutex); | |
- std::fstream f("fclstatistics.log", std::fstream::out | std::fstream::app); | |
+ std::fstream f(fileprefix+"fclstatistics.log", std::fstream::out | std::fstream::app); | |
FOREACH(ittiming, timings) { | |
f << ittiming->first; | |
size_t maxTimingCount = 0; | |
@@ -87,6 +87,11 @@ public: | |
#endif | |
} | |
+ void Reset() { | |
+ currentTimings.clear(); | |
+ timings.clear(); | |
+ } | |
+ | |
void AddTimepoint() { | |
currentTimings.push_back(std::chrono::high_resolution_clock::now()); | |
} | |
@@ -122,7 +127,11 @@ typedef boost::shared_ptr<FCLStatistics> FCLStatisticsPtr; | |
#define ADD_TIMING(statistics) statistics->AddTimepoint() | |
-#define DISPLAY(statistics) statistics->DisplayAll() | |
+#define DISPLAY(statistics, fileprefix) statistics->Display(fileprefix) | |
+ | |
+#define DISPLAY_ALL(statistics) statistics->DisplayAll() | |
+ | |
+#define RESET_TIMINGS(statistics) statistics->Reset() | |
} // fclrave | |
@@ -136,7 +145,9 @@ class FCLStatistics { | |
#define SETUP_STATISTICS(statistics, userdatakey, id) do {} while(false) | |
#define START_TIMING(statistics, label) do {} while(false) | |
#define ADD_TIMING(statistics) do {} while(false) | |
-#define DISPLAY(statistics) do {} while(false) | |
+#define DISPLAY(statistics, fileprefix) do {} while(false) | |
+#define DISPLAY_ALL(statistics) do {} while(false) | |
+#define RESET_TIMINGS(statistics) do {} while(false) | |
} | |
#endif | |
diff --git a/plugins/rplanners/parabolicsmoother.cpp b/plugins/rplanners/parabolicsmoother.cpp | |
index 751c15209..48adeffc6 100644 | |
--- a/plugins/rplanners/parabolicsmoother.cpp | |
+++ b/plugins/rplanners/parabolicsmoother.cpp | |
@@ -22,7 +22,8 @@ | |
#include "ParabolicPathSmooth/DynamicPath.h" | |
#include "trajectoryretimer.h" // _(msgid) | |
-// #define SMOOTHER1_TIMING_DEBUG | |
+//#define OPENRAVE_TIMING_DEBUGGING | |
+#define SMOOTHER1_TIMING_DEBUG | |
// #define SMOOTHER1_PROGRESS_DEBUG | |
namespace rplanners { | |
@@ -509,6 +510,11 @@ public: | |
// no idea what a good mintimestep is... _parameters->_fStepLength*0.5? | |
//numshortcuts = dynamicpath.Shortcut(parameters->_nMaxIterations,_feasibilitychecker,this, parameters->_fStepLength*0.99); | |
#ifdef SMOOTHER1_TIMING_DEBUG | |
+ //CollisionCheckerBasePtr pcollsionchecker = GetEnv()->GetCollisionChecker(); | |
+ //if (!!pcollsionchecker && pcollsionchecker->SupportsCommand("ResetStatistics")) { | |
+ // std::string sout; | |
+ // pcollsionchecker->SendCommand(sout, "ResetStatistics"); | |
+ //} | |
_tShortcutStart = utils::GetMicroTime(); | |
#endif | |
if (!_usingNewHeuristics) { | |
@@ -519,6 +525,10 @@ public: | |
} | |
#ifdef SMOOTHER1_TIMING_DEBUG | |
_tShortcutEnd = utils::GetMicroTime(); | |
+ //if (!!pcollsionchecker && pcollsionchecker->SupportsCommand("DisplayStatistics")) { | |
+ // std::string sout; | |
+ // pcollsionchecker->SendCommand(sout, "DisplayStatistics parabolicsmoother_shortcutting"); | |
+ //} | |
#endif | |
if( numshortcuts < 0 ) { | |
return OPENRAVE_PLANNER_STATUS(str(boost::format("env=%d, Planning was interrupted")%GetEnv()->GetId()), PS_Interrupted); | |
@@ -529,10 +539,9 @@ public: | |
} | |
RAVELOG_DEBUG_FORMAT("env=%d, after shortcutting: duration %.15e -> %.15e, diff = %.15e", GetEnv()->GetId()%dummyDur1%dummyDur2%(dummyDur1 - dummyDur2)); | |
-#ifdef OPENRAVE_TIMING_DEBUGGING | |
- RAVELOG_INFO_FORMAT("env=%d, calling checkmanipconstraints %d times, using %.15e sec. = %.15e sec./call", GetEnv()->GetId()%ncheckmanipconstraints%checkmaniptime%(ncheckmanipconstraints == 0 ? 0 : checkmaniptime/ncheckmanipconstraints)); | |
-#endif | |
- | |
+// #ifdef OPENRAVE_TIMING_DEBUGGING | |
+// RAVELOG_INFO_FORMAT("env=%d, calling checkmanipconstraints %d times, using %.15e sec. = %.15e sec./call", GetEnv()->GetId()%ncheckmanipconstraints%checkmaniptime%(ncheckmanipconstraints == 0 ? 0 : checkmaniptime/ncheckmanipconstraints)); | |
+// #endif | |
++_progress._iteration; | |
if( _CallCallbacks(_progress) == PA_Interrupt ) { | |
return OPENRAVE_PLANNER_STATUS(str(boost::format("env=%d, Planning was interrupted")%GetEnv()->GetId()), PS_Interrupted); | |
@@ -581,6 +590,7 @@ public: | |
temprampsnd[0] = rampnd; | |
// double-check the current ramps, ignore first and last ramps since they connect to the initial and goal positions, and those most likely they cannot be fixed . | |
if(!rampnd.constraintchecked ) { | |
+ RAVELOG_WARN_FORMAT("double check ramp[%d/%d]!!!!", irampindex%dynamicpath.ramps.size()); | |
//(irampindex > 0 && irampindex+1 < dynamicpath.ramps.size()) | |
rampndtrimmed = rampnd; | |
bool bTrimmed = false; | |
@@ -769,6 +779,8 @@ public: | |
if( _CallCallbacks(_progress) == PA_Interrupt ) { | |
return OPENRAVE_PLANNER_STATUS(str(boost::format("env=%d, Planning was interrupted")%GetEnv()->GetId()), PS_Interrupted); | |
} | |
+ } else { | |
+ //RAVELOG_DEBUG_FORMAT("no double check of ramp[%d/%d]", irampindex%dynamicpath.ramps.size()); | |
} | |
FOREACH(itrampnd2, temprampsnd) { | |
@@ -1409,6 +1421,12 @@ protected: | |
dReal cutoffRatio = 0;//1e-3; | |
#ifdef OPENRAVE_TIMING_DEBUGGING | |
uint32_t tshortcutstart = utils::GetMicroTime(); | |
+ dReal interpolationtime = 0.0; | |
+ uint64_t ninterpolations = 0; | |
+ dReal checktime = 0.0; | |
+ uint64_t nchecks = 0; | |
+ uint64_t nslowdownloops = 0; | |
+ dReal slowdownlooptime = 0.0; | |
#endif | |
int iters=0; | |
for(iters=0; iters<numIters; iters++) { | |
@@ -1502,17 +1520,17 @@ protected: | |
dReal fcurmult = fstarttimemult; | |
#ifdef OPENRAVE_TIMING_DEBUGGING | |
- tloopstart = utils::GetMicroTime(); | |
+ uint64_t tloopstart = utils::GetMicroTime(); | |
#endif | |
size_t islowdowntry = 0; | |
bool bShortcutTimeExceeded = false; | |
for(islowdowntry = 0; islowdowntry < 4; ++islowdowntry ) { | |
#ifdef OPENRAVE_TIMING_DEBUGGING | |
- tinterpstart = utils::GetMicroTime(); | |
+ uint64_t tinterpstart = utils::GetMicroTime(); | |
#endif | |
bool res=ParabolicRamp::SolveMinTime(x0, dx0, x1, dx1, accellimits, vellimits, _parameters->_vConfigLowerLimit, _parameters->_vConfigUpperLimit, intermediate, _parameters->_multidofinterp); | |
#ifdef OPENRAVE_TIMING_DEBUGGING | |
- tinterpend = utils::GetMicroTime(); | |
+ uint64_t tinterpend = utils::GetMicroTime(); | |
interpolationtime += 0.000001f*(float)(tinterpend - tinterpstart); | |
ninterpolations += 1; | |
#endif | |
@@ -1565,11 +1583,11 @@ protected: | |
iIterProgress += 0x10; | |
#ifdef OPENRAVE_TIMING_DEBUGGING | |
- tcheckstart = utils::GetMicroTime(); | |
+ uint64_t tcheckstart = utils::GetMicroTime(); | |
#endif | |
retcheck = _feasibilitychecker.Check2(intermediate.ramps[iramp], 0xffff|CFO_FromTrajectorySmoother, outramps); | |
#ifdef OPENRAVE_TIMING_DEBUGGING | |
- tcheckend = utils::GetMicroTime(); | |
+ uint64_t tcheckend = utils::GetMicroTime(); | |
checktime += 0.000001f*(float)(tcheckend - tcheckstart); | |
nchecks += 1; | |
#endif | |
@@ -1749,7 +1767,7 @@ protected: | |
} | |
#ifdef OPENRAVE_TIMING_DEBUGGING | |
nslowdownloops += (islowdowntry + 1); | |
- tloopend = utils::GetMicroTime(); | |
+ uint64_t tloopend = utils::GetMicroTime(); | |
slowdownlooptime += 0.000001f*(float)(tloopend - tloopstart); | |
#endif | |
if (bShortcutTimeExceeded) { | |
@@ -2001,6 +2019,12 @@ protected: | |
dReal currentBestScore = 1.0; | |
#ifdef OPENRAVE_TIMING_DEBUGGING | |
uint32_t tshortcutstart = utils::GetMicroTime(); | |
+ dReal interpolationtime = 0.0; | |
+ uint64_t ninterpolations = 0; | |
+ dReal checktime = 0.0; | |
+ uint64_t nchecks = 0; | |
+ uint64_t nslowdownloops = 0; | |
+ dReal slowdownlooptime = 0.0; | |
#endif | |
int iters = 0; | |
// For special shortcut | |
diff --git a/python/bindings/CMakeLists.txt b/python/bindings/CMakeLists.txt | |
index b579654f7..f1005692a 100644 | |
--- a/python/bindings/CMakeLists.txt | |
+++ b/python/bindings/CMakeLists.txt | |
@@ -84,9 +84,9 @@ if(USE_PYBIND11_PYTHON_BINDINGS) | |
add_library(pyANN_int SHARED pyann.cpp bindings.cpp) | |
include_directories(${ANN_INCLUDE_DIR}) | |
# stdc++ has to be included before opengl libraries due to some ATI bug (http://wiki.fifengine.de/ Segfault_in_cxa_allocate_exception#Workaround) | |
- target_link_libraries(pyANN_int PUBLIC ${OPENRAVEPY_LINKED_LIBRARIES} ANN PRIVATE boost_assertion_failed) | |
+ target_link_libraries(pyANN_int PUBLIC ${OPENRAVEPY_LINKED_LIBRARIES} libopenrave libopenrave-core ANN PRIVATE boost_assertion_failed) | |
set_target_properties(pyANN_int PROPERTIES PREFIX "" COMPILE_FLAGS "${ANN_CFLAGS} ${OPENRAVEPY_COMPILE_FLAGS}") | |
- add_dependencies(pyANN_int ANN) | |
+ add_dependencies(pyANN_int ANN libopenrave libopenrave-core) | |
if( WIN32 ) | |
set_target_properties(pyANN_int PROPERTIES SUFFIX ".pyd") | |
if( MSVC ) | |
@@ -104,9 +104,9 @@ if(USE_PYBIND11_PYTHON_BINDINGS) | |
if( CONVEXDECOMPOSITION_FOUND ) | |
add_library(convexdecompositionpy SHARED convexdecompositionpy.cpp bindings.cpp) | |
include_directories(${CONVEXDECOMPOSITION_INCLUDE_DIR}) | |
- target_link_libraries(convexdecompositionpy PUBLIC ${OPENRAVEPY_LINKED_LIBRARIES} convexdecomposition PRIVATE boost_assertion_failed) | |
+ target_link_libraries(convexdecompositionpy PUBLIC ${OPENRAVEPY_LINKED_LIBRARIES} convexdecomposition libopenrave libopenrave-core PRIVATE boost_assertion_failed) | |
set_target_properties(convexdecompositionpy PROPERTIES PREFIX "" COMPILE_FLAGS "${CONVEXDECOMPOSITION_CFLAGS} ${OPENRAVEPY_COMPILE_FLAGS}" ) | |
- add_dependencies(convexdecompositionpy convexdecomposition) | |
+ add_dependencies(convexdecompositionpy convexdecomposition libopenrave libopenrave-core) | |
if( WIN32 ) | |
set_target_properties(convexdecompositionpy PROPERTIES SUFFIX ".pyd") | |
if( MSVC ) | |
@@ -154,7 +154,7 @@ elseif( Boost_PYTHON_FOUND AND Boost_THREAD_FOUND ) | |
include_directories(${CMAKE_CURRENT_BINARY_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/include ${PYTHON_INCLUDE_DIRS} ${OPENRAVE_INCLUDE_LOCAL_DIRS} ${OPENRAVE_CORE_INCLUDE_LOCAL_DIRS}) | |
# libraries to link | |
- set(OPENRAVEPY_LINKED_LIBRARIES "${STDC_LIBRARY} ${PYTHON_LIBRARIES} ${Boost_PYTHON_LIBRARY} ${Boost_THREAD_LIBRARY}") | |
+ set(OPENRAVEPY_LINKED_LIBRARIES "${STDC_LIBRARY} ${PYTHON_LIBRARIES} ${Boost_PYTHON_LIBRARY} ${Boost_THREAD_LIBRARY} ${Boost_SYSTEM_LIBRARY}") | |
if( CLOCK_GETTIME_FOUND ) | |
set(OPENRAVEPY_LINKED_LIBRARIES ${OPENRAVEPY_LINKED_LIBRARIES} rt) | |
endif() | |
@@ -191,9 +191,9 @@ elseif( Boost_PYTHON_FOUND AND Boost_THREAD_FOUND ) | |
if( ANN_FOUND ) | |
add_library(pyANN_int SHARED pyann.cpp bindings.cpp) | |
include_directories(${ANN_INCLUDE_DIR}) | |
- target_link_libraries(pyANN_int PUBLIC ${OPENRAVEPY_LINKED_LIBRARIES} libopenrave ANN PRIVATE boost_assertion_failed) | |
+ target_link_libraries(pyANN_int PUBLIC ${OPENRAVEPY_LINKED_LIBRARIES} libopenrave libopenrave-core ANN PRIVATE boost_assertion_failed) | |
set_target_properties(pyANN_int PROPERTIES PREFIX "" COMPILE_FLAGS "${ANN_CFLAGS}" ) | |
- add_dependencies(pyANN_int ANN) | |
+ add_dependencies(pyANN_int ANN libopenrave libopenrave-core) | |
if( WIN32 ) | |
set_target_properties(pyANN_int PROPERTIES SUFFIX ".pyd") | |
if( MSVC ) | |
@@ -210,9 +210,9 @@ elseif( Boost_PYTHON_FOUND AND Boost_THREAD_FOUND ) | |
if( CONVEXDECOMPOSITION_FOUND ) | |
add_library(convexdecompositionpy SHARED convexdecompositionpy.cpp bindings.cpp) | |
include_directories(${CONVEXDECOMPOSITION_INCLUDE_DIR}) | |
- target_link_libraries(convexdecompositionpy PUBLIC ${OPENRAVEPY_LINKED_LIBRARIES} libopenrave convexdecomposition PRIVATE boost_assertion_failed) | |
+ target_link_libraries(convexdecompositionpy PUBLIC ${OPENRAVEPY_LINKED_LIBRARIES} libopenrave libopenrave-core convexdecomposition PRIVATE boost_assertion_failed) | |
set_target_properties(convexdecompositionpy PROPERTIES PREFIX "" COMPILE_FLAGS "${CONVEXDECOMPOSITION_CFLAGS}" ) | |
- add_dependencies(convexdecompositionpy convexdecomposition) | |
+ add_dependencies(convexdecompositionpy convexdecomposition libopenrave libopenrave-core) | |
if( WIN32 ) | |
set_target_properties(convexdecompositionpy PROPERTIES SUFFIX ".pyd") | |
if( MSVC ) | |
diff --git a/src/collada_robots b/src/collada_robots | |
--- a/src/collada_robots | |
+++ b/src/collada_robots | |
@@ -1 +1 @@ | |
-Subproject commit 98c0760b8cdff9c4cd754752184421f21ff19946 | |
+Subproject commit 98c0760b8cdff9c4cd754752184421f21ff19946-dirty | |
diff --git a/src/libopenrave/planningutils.cpp b/src/libopenrave/planningutils.cpp | |
index a4f98952a..1512ba9dd 100644 | |
--- a/src/libopenrave/planningutils.cpp | |
+++ b/src/libopenrave/planningutils.cpp | |
@@ -2027,8 +2027,8 @@ void GetDHParameters(std::vector<DHParameter>& vparameters, KinBodyConstPtr pbod | |
Vector vlocalx(-vlocalaxis.y,vlocalaxis.x,0); | |
dReal fsinalpha = RaveSqrt(vlocalx.lengthsqr3()); | |
itdh->alpha = RaveAtan2(fsinalpha, vlocalaxis.z); | |
- if( itdh->alpha < 10*g_fEpsilon ) { | |
- // axes are parallel | |
+ if( itdh->alpha < 10*g_fEpsilon || (RaveFabs(itdh->alpha) - PI) < 10*g_fEpsilon) { | |
+ // axes are parallel, or in a flipped manner | |
if( vlocalanchor.lengthsqr2() > 10*g_fEpsilon*g_fEpsilon ) { | |
vlocalx.x = vlocalanchor.x; | |
vlocalx.y = vlocalanchor.y; | |
@@ -2187,7 +2187,7 @@ int DynamicsCollisionConstraint::_CheckState(const std::vector<dReal>& vdofveloc | |
return CFO_CheckUserConstraints; | |
} | |
} | |
- if( options & CFO_CheckTimeBasedConstraints ) { | |
+ if( (options & CFO_CheckTimeBasedConstraints) ) { | |
// check dynamics | |
FOREACHC(itbody, _listCheckBodies) { | |
KinBodyPtr pbody = *itbody; | |
@@ -2925,7 +2925,7 @@ int DynamicsCollisionConstraint::Check(const std::vector<dReal>& q0, const std:: | |
bComputeNewStep = false; | |
} | |
prevtimestep = timestep; // have to always update since it serves as the basis for the next timestep chosen | |
- } | |
+ } // end of while | |
if( RaveFabs(fStep-fLargestStep) > RaveFabs(fLargestStepDelta) ) { | |
RAVELOG_WARN_FORMAT("fStep (%.15e) did not reach fLargestStep (%.15e). %.15e > %.15e", fStep%fLargestStep%RaveFabs(fStep-fLargestStep)%fLargestStepDelta); | |
if( !!filterreturn ) { |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment