From abb5806676eea286083229ccc64f67954a13bdd7 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Wed, 13 Mar 2024 12:14:12 +0900 Subject: [PATCH 01/14] add SetTextSize viewer function for adjusting HUD text size --- include/openrave/viewer.h | 2 ++ plugins/qtcoinrave/item.cpp | 3 ++- plugins/qtcoinrave/item.h | 4 ++++ plugins/qtcoinrave/qtcoinviewer.cpp | 10 +++++++++- plugins/qtcoinrave/qtcoinviewer.h | 2 ++ plugins/qtosgrave/osgviewerwidget.cpp | 16 +++++++++++++--- plugins/qtosgrave/osgviewerwidget.h | 4 ++++ plugins/qtosgrave/qtosgviewer.cpp | 5 +++++ plugins/qtosgrave/qtosgviewer.h | 3 +++ python/bindings/openravepy_viewer.cpp | 4 ++++ 10 files changed, 48 insertions(+), 5 deletions(-) diff --git a/include/openrave/viewer.h b/include/openrave/viewer.h index cce8deec40..e048d8f62b 100644 --- a/include/openrave/viewer.h +++ b/include/openrave/viewer.h @@ -176,6 +176,8 @@ class OPENRAVE_API ViewerBase : public InterfaceBase virtual void SetUserText(const std::string& userText) OPENRAVE_DUMMY_IMPLEMENTATION; + virtual void SetTextSize(double size) OPENRAVE_DUMMY_IMPLEMENTATION; + /// \brief controls showing the viewer. /// /// \param showtype If zero, will hide all viewers. If != 0, should show viewers (dependent on plugin could have different meanings) diff --git a/plugins/qtcoinrave/item.cpp b/plugins/qtcoinrave/item.cpp index d8030d5f08..5d2d94876a 100644 --- a/plugins/qtcoinrave/item.cpp +++ b/plugins/qtcoinrave/item.cpp @@ -533,6 +533,7 @@ KinBody::LinkPtr KinBodyItem::GetLinkFromIv(SoNode* plinknode) const RobotItem::RobotItem(QtCoinViewerPtr viewer, RobotBasePtr robot, ViewGeometry viewgeom) : KinBodyItem(viewer, robot, viewgeom), _probot(robot) { + _textSize = 18; } void RobotItem::Load() @@ -694,7 +695,7 @@ void RobotItem::CreateAxis(RobotItem::EE& ee, const string& name, const Vector* SoFont* pfont = new SoFont(); pfont->name = "Courier:Bold"; - pfont->size = 18; + pfont->size = _textSize; ptextsep->addChild(pfont); SoText2 * ptext = new SoText2(); diff --git a/plugins/qtcoinrave/item.h b/plugins/qtcoinrave/item.h index 7e8806eb6d..a2893584d8 100644 --- a/plugins/qtcoinrave/item.h +++ b/plugins/qtcoinrave/item.h @@ -233,10 +233,14 @@ class RobotItem : public KinBodyItem } virtual void SetGrab(bool bGrab, bool bUpdate=true); virtual void Load(); + virtual void SetTextSize(double size) { + _textSize = size; + } private: void CreateAxis(EE& ee, const string& name, const Vector* pdirection=NULL); std::vector< EE > _vEndEffectors, _vAttachedSensors; RobotBasePtr _probot; + double _textSize; }; typedef boost::shared_ptr RobotItemPtr; typedef boost::shared_ptr RobotItemConstPtr; diff --git a/plugins/qtcoinrave/qtcoinviewer.cpp b/plugins/qtcoinrave/qtcoinviewer.cpp index ccd78c6a76..7c4d62734e 100644 --- a/plugins/qtcoinrave/qtcoinviewer.cpp +++ b/plugins/qtcoinrave/qtcoinviewer.cpp @@ -640,6 +640,12 @@ void QtCoinViewer::SetUserText(const string& userText) _userText = userText; } +void QtCoinViewer::SetTextSize(double size) +{ + _textSize = size; + UpdateFromModel(); +} + bool QtCoinViewer::LoadModel(const string& pfilename) { SoInput mySceneInput; @@ -3015,7 +3021,9 @@ void QtCoinViewer::UpdateFromModel() } if( pbody->IsRobot() ) { - pitem = boost::shared_ptr(new RobotItem(shared_viewer(), RaveInterfaceCast(pbody), _viewGeometryMode),ITEM_DELETER); + boost::shared_ptr probot = boost::shared_ptr(new RobotItem(shared_viewer(), RaveInterfaceCast(pbody), _viewGeometryMode),ITEM_DELETER); + probot->SetTextSize(_textSize); + pitem = probot; } else { pitem = boost::shared_ptr(new KinBodyItem(shared_viewer(), pbody, _viewGeometryMode),ITEM_DELETER); diff --git a/plugins/qtcoinrave/qtcoinviewer.h b/plugins/qtcoinrave/qtcoinviewer.h index 1f8c04166d..b3978b55eb 100644 --- a/plugins/qtcoinrave/qtcoinviewer.h +++ b/plugins/qtcoinrave/qtcoinviewer.h @@ -132,6 +132,7 @@ class QtCoinViewer : public QMainWindow, public ViewerBase return _name; } virtual void SetUserText(const string& userText); + virtual void SetTextSize(double size); virtual bool LoadModel(const string& filename); @@ -421,6 +422,7 @@ public slots: std::string _name; std::string _userText; + double _textSize; std::map _mapbodies; ///< all the bodies created ItemPtr _pSelectedItem; ///< the currently selected item diff --git a/plugins/qtosgrave/osgviewerwidget.cpp b/plugins/qtosgrave/osgviewerwidget.cpp index 837fc14420..87da5708bf 100644 --- a/plugins/qtosgrave/osgviewerwidget.cpp +++ b/plugins/qtosgrave/osgviewerwidget.cpp @@ -644,6 +644,7 @@ QOSGViewerWidget::QOSGViewerWidget(EnvironmentBasePtr penv, const std::string& u _osgWorldAxis->addChild(CreateOSGXYZAxes(32.0, 2.0)); _vecTextScreenOffset = osg::Vec2(10.0, 0.0); + _hudTextSize = 18.0; if( !!_osgCameraHUD ) { // in order to get the axes to render without lighting: @@ -1163,11 +1164,20 @@ void QOSGViewerWidget::SetViewport(int width, int height) osg::Camera *hudcamera = _osghudview->getCamera(); hudcamera->setViewport(0, 0, width * scale, height * scale); - double textheight = 18*scale; + SetHUDTextSize(_hudTextSize); + _UpdateHUDAxisTransform(width, height); +} + +void QOSGViewerWidget::SetHUDTextSize(double size) +{ + _hudTextSize = size; + + float scale = this->devicePixelRatio(); + double textheight = _hudTextSize*scale; _osgHudText->setCharacterSize(textheight); _osgHudText->setFontResolution(textheight, textheight); + SetHUDTextOffset(_vecTextScreenOffset.x(), _vecTextScreenOffset.y()); - _UpdateHUDAxisTransform(width, height); } osg::Vec2 QOSGViewerWidget::GetHUDTextOffset() @@ -1180,7 +1190,7 @@ void QOSGViewerWidget::SetHUDTextOffset(double xOffset, double yOffset) _vecTextScreenOffset.set(xOffset, yOffset); double scale = this->devicePixelRatio(); - double textheight = 18*scale; + double textheight = _hudTextSize*scale; _osgHudText->setPosition( osg::Vec3( -_osgview->getCamera()->getViewport()->width() / 2 + _vecTextScreenOffset.x(), diff --git a/plugins/qtosgrave/osgviewerwidget.h b/plugins/qtosgrave/osgviewerwidget.h index 35e8a2c0a8..aff20ab564 100644 --- a/plugins/qtosgrave/osgviewerwidget.h +++ b/plugins/qtosgrave/osgviewerwidget.h @@ -128,6 +128,9 @@ class QOSGViewerWidget : public QOpenGLWidget /// \brief called when the qt window size changes void SetViewport(int width, int height); + /// \brief sets the HUD text size, scaled off of devicePixelRatio + void SetHUDTextSize(double size); + /// \brief gets the screen offset of HUD text (default with no control buttons is (10.0, 0.0)) osg::Vec2 GetHUDTextOffset(); @@ -345,6 +348,7 @@ class QOSGViewerWidget : public QOpenGLWidget /// causing getProjectionMatrixAsXXX to return negative /// values. Therefore, we manage zNear ourselves double _currentOrthoFrustumSize; ///< coordinate for the right vertical clipping plane + double _hudTextSize; ///< size of HUD text, scaled off of devicePixelRatio void GetSwitchedButtonValue(unsigned int &button); diff --git a/plugins/qtosgrave/qtosgviewer.cpp b/plugins/qtosgrave/qtosgviewer.cpp index 18b269acec..9f3b4423d8 100644 --- a/plugins/qtosgrave/qtosgviewer.cpp +++ b/plugins/qtosgrave/qtosgviewer.cpp @@ -2041,6 +2041,11 @@ void QtOSGViewer::SetUserText(const string& userText) _posgWidget->SetUserHUDText(userText); } +void QtOSGViewer::SetTextSize(double size) +{ + _posgWidget->SetHUDTextSize(size); +} + bool QtOSGViewer::LoadModel(const string& filename) { if( filename == "") { diff --git a/plugins/qtosgrave/qtosgviewer.h b/plugins/qtosgrave/qtosgviewer.h index c02aebc140..c2ea2088c6 100644 --- a/plugins/qtosgrave/qtosgviewer.h +++ b/plugins/qtosgrave/qtosgviewer.h @@ -113,6 +113,9 @@ class QtOSGViewer : public QMainWindow, public ViewerBase /// \brief Set User-defined text to be displayed in the viewer window virtual void SetUserText(const string& userText); + /// \brief Set size of text to be displayed in the viewer window + virtual void SetTextSize(double size); + /// \brief notified when a body has been removed from the environment virtual void RemoveKinBody(KinBodyPtr pbody) { if( !!pbody ) { diff --git a/python/bindings/openravepy_viewer.cpp b/python/bindings/openravepy_viewer.cpp index 2dbbc41934..dcda193b2d 100644 --- a/python/bindings/openravepy_viewer.cpp +++ b/python/bindings/openravepy_viewer.cpp @@ -258,6 +258,9 @@ class PyViewerBase : public PyInterfaceBase void SetUserText(const std::string &userText) { _pviewer->SetUserText(userText); } + void SetTextSize(const double size) { + _pviewer->SetTextSize(size); + } std::string GetName() { return _pviewer->GetName(); } @@ -386,6 +389,7 @@ void init_openravepy_viewer() .def("Show",&PyViewerBase::Show, PY_ARGS("showtype") DOXY_FN(ViewerBase,Show)) .def("SetTitle",&PyViewerBase::SetName, PY_ARGS("title") DOXY_FN(ViewerBase,SetName)) .def("SetUserText",&PyViewerBase::SetUserText, PY_ARGS("userText") DOXY_FN(ViewerBase,SetUserText)) + .def("SetTextSize",&PyViewerBase::SetTextSize, PY_ARGS("size") DOXY_FN(ViewerBase,SetTextSize)) .def("SetName",&PyViewerBase::SetName, PY_ARGS("title") DOXY_FN(ViewerBase,SetName)) .def("GetName",&PyViewerBase::GetName, DOXY_FN(ViewerBase,GetName)) .def("RegisterCallback",&PyViewerBase::RegisterCallback, PY_ARGS("properties", "callback") DOXY_FN(ViewerBase,RegisterItemSelectionCallback)) From 48961ae71d5f24f0a71b54e0880a993bab8c29ea Mon Sep 17 00:00:00 2001 From: Jonathan Date: Wed, 13 Mar 2024 13:50:33 +0900 Subject: [PATCH 02/14] Guard against negative text size in SetTextSize methods --- plugins/qtcoinrave/qtcoinviewer.cpp | 6 ++++-- plugins/qtosgrave/osgviewerwidget.cpp | 14 ++++++++------ 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/plugins/qtcoinrave/qtcoinviewer.cpp b/plugins/qtcoinrave/qtcoinviewer.cpp index 7c4d62734e..a1b06ff197 100644 --- a/plugins/qtcoinrave/qtcoinviewer.cpp +++ b/plugins/qtcoinrave/qtcoinviewer.cpp @@ -642,8 +642,10 @@ void QtCoinViewer::SetUserText(const string& userText) void QtCoinViewer::SetTextSize(double size) { - _textSize = size; - UpdateFromModel(); + if ( size >= 0 ) { + _textSize = size; + UpdateFromModel(); + } } bool QtCoinViewer::LoadModel(const string& pfilename) diff --git a/plugins/qtosgrave/osgviewerwidget.cpp b/plugins/qtosgrave/osgviewerwidget.cpp index 87da5708bf..def827498d 100644 --- a/plugins/qtosgrave/osgviewerwidget.cpp +++ b/plugins/qtosgrave/osgviewerwidget.cpp @@ -1170,14 +1170,16 @@ void QOSGViewerWidget::SetViewport(int width, int height) void QOSGViewerWidget::SetHUDTextSize(double size) { - _hudTextSize = size; + if ( size >= 0 ) { + _hudTextSize = size; - float scale = this->devicePixelRatio(); - double textheight = _hudTextSize*scale; - _osgHudText->setCharacterSize(textheight); - _osgHudText->setFontResolution(textheight, textheight); + float scale = this->devicePixelRatio(); + double textheight = _hudTextSize*scale; + _osgHudText->setCharacterSize(textheight); + _osgHudText->setFontResolution(textheight, textheight); - SetHUDTextOffset(_vecTextScreenOffset.x(), _vecTextScreenOffset.y()); + SetHUDTextOffset(_vecTextScreenOffset.x(), _vecTextScreenOffset.y()); + } } osg::Vec2 QOSGViewerWidget::GetHUDTextOffset() From e1ff951d9f0c8107441fd7f4ebcf82c0c3b7b06e Mon Sep 17 00:00:00 2001 From: Jonathan Date: Wed, 13 Mar 2024 15:36:46 +0900 Subject: [PATCH 03/14] Bump minor version and update changelog --- CMakeLists.txt | 4 ++-- docs/source/changelog.rst | 5 +++++ 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c95d70540..7db597fef3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,8 +4,8 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE ) # Define here the needed parameters set (OPENRAVE_VERSION_MAJOR 0) -set (OPENRAVE_VERSION_MINOR 141) -set (OPENRAVE_VERSION_PATCH 1) +set (OPENRAVE_VERSION_MINOR 142) +set (OPENRAVE_VERSION_PATCH 0) 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 08786650dc..2a69ffe0b6 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -3,6 +3,11 @@ ChangeLog ######### +Version 0.142.0 +=============== + +* Add ViewerBase::SetUserText to customize HUD text size + Version 0.141.1 =============== From 7d0efe17b522b71bf84d37a7f80303a964a3c590 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Wed, 13 Mar 2024 19:53:38 +0900 Subject: [PATCH 04/14] Fix qtcoinrave textSize implementation to target message nodes, add breakpoints for textSize and message translations --- plugins/qtcoinrave/item.cpp | 5 +- plugins/qtcoinrave/item.h | 6 +- plugins/qtcoinrave/qtcoinviewer.cpp | 108 ++++++++++++++++++++++---- plugins/qtcoinrave/qtcoinviewer.h | 8 +- plugins/qtosgrave/osgviewerwidget.cpp | 2 +- 5 files changed, 106 insertions(+), 23 deletions(-) diff --git a/plugins/qtcoinrave/item.cpp b/plugins/qtcoinrave/item.cpp index 5d2d94876a..06bdb05254 100644 --- a/plugins/qtcoinrave/item.cpp +++ b/plugins/qtcoinrave/item.cpp @@ -533,7 +533,6 @@ KinBody::LinkPtr KinBodyItem::GetLinkFromIv(SoNode* plinknode) const RobotItem::RobotItem(QtCoinViewerPtr viewer, RobotBasePtr robot, ViewGeometry viewgeom) : KinBodyItem(viewer, robot, viewgeom), _probot(robot) { - _textSize = 18; } void RobotItem::Load() @@ -695,7 +694,7 @@ void RobotItem::CreateAxis(RobotItem::EE& ee, const string& name, const Vector* SoFont* pfont = new SoFont(); pfont->name = "Courier:Bold"; - pfont->size = _textSize; + pfont->size = 18; ptextsep->addChild(pfont); SoText2 * ptext = new SoText2(); @@ -769,4 +768,4 @@ bool RobotItem::UpdateFromModel(const vector& vjointvalues, const vector< } return true; -} +} \ No newline at end of file diff --git a/plugins/qtcoinrave/item.h b/plugins/qtcoinrave/item.h index a2893584d8..bee07117f7 100644 --- a/plugins/qtcoinrave/item.h +++ b/plugins/qtcoinrave/item.h @@ -233,14 +233,10 @@ class RobotItem : public KinBodyItem } virtual void SetGrab(bool bGrab, bool bUpdate=true); virtual void Load(); - virtual void SetTextSize(double size) { - _textSize = size; - } private: void CreateAxis(EE& ee, const string& name, const Vector* pdirection=NULL); std::vector< EE > _vEndEffectors, _vAttachedSensors; RobotBasePtr _probot; - double _textSize; }; typedef boost::shared_ptr RobotItemPtr; typedef boost::shared_ptr RobotItemConstPtr; @@ -252,4 +248,4 @@ BOOST_TYPEOF_REGISTER_TYPE(RobotItem) BOOST_TYPEOF_REGISTER_TYPE(RobotItem::EE) #endif -#endif // ITEM_H +#endif // ITEM_H \ No newline at end of file diff --git a/plugins/qtcoinrave/qtcoinviewer.cpp b/plugins/qtcoinrave/qtcoinviewer.cpp index a1b06ff197..1fabe908d1 100644 --- a/plugins/qtcoinrave/qtcoinviewer.cpp +++ b/plugins/qtcoinrave/qtcoinviewer.cpp @@ -257,9 +257,14 @@ void QtCoinViewer::_InitConstructor(std::istream& sinput) // add the message texts SoSeparator* pmsgsep = new SoSeparator(); - SoTranslation* pmsgtrans0 = new SoTranslation(); - pmsgtrans0->translation.setValue(SbVec3f(-0.978f,0.93f,0)); - pmsgsep->addChild(pmsgtrans0); + + _messagefont = new SoFont(); + _messagefont->size = 18; + pmsgsep->addChild(_messagefont); + + _messageBaseTranslation = new SoTranslation(); + _messageBaseTranslation->translation.setValue(SbVec3f(-0.978f,0.874f,0)); + pmsgsep->addChild(_messageBaseTranslation); SoBaseColor* pcolor0 = new SoBaseColor(); pcolor0->rgb.setValue(0.0f,0.0f,0.0f); pmsgsep->addChild(pcolor0); @@ -267,7 +272,7 @@ void QtCoinViewer::_InitConstructor(std::istream& sinput) pmsgsep->addChild(_messageNodes[0]); _messageShadowTranslation = new SoTranslation(); - _messageShadowTranslation->translation.setValue(SbVec3f(-0.002f,0.032f,0)); + _messageShadowTranslation->translation.setValue(SbVec3f(-0.002f,0.0540f,0)); pmsgsep->addChild(_messageShadowTranslation); SoBaseColor* pcolor1 = new SoBaseColor(); pcolor1->rgb.setValue(0.99f,0.99f,0.99f); @@ -640,12 +645,93 @@ void QtCoinViewer::SetUserText(const string& userText) _userText = userText; } +class SetTextSizeMessage : public QtCoinViewer::EnvMessage +{ +public: + SetTextSizeMessage(QtCoinViewerPtr pviewer, void** ppreturn, double size) + : EnvMessage(pviewer, ppreturn, false), _textSize(size) { + } + + virtual void viewerexecute() { + QtCoinViewerPtr pviewer = _pviewer.lock(); + if( !pviewer ) { + return; + } + pviewer->_SetTextSize(_textSize); + EnvMessage::viewerexecute(); + } + +private: + double _textSize; +}; + void QtCoinViewer::SetTextSize(double size) { - if ( size >= 0 ) { - _textSize = size; - UpdateFromModel(); + if (_timerSensor->isScheduled() && _bUpdateEnvironment) { + EnvMessagePtr pmsg(new SetTextSizeMessage(shared_viewer(), (void**)NULL, size)); + pmsg->callerexecute(false); + } +} + +void QtCoinViewer::_SetTextSize(double size) +{ + if ( size > 0 ) { + // TODO: use a font that does not rely on hardcoded breakpoints + // move down to next text size breakpoint so that message shadow aligns nicely + _messagefont->size = _GetTextBaseSize(size); + // adjust the text offsets + _messageBaseTranslation->translation.setValue(_GetMessageBaseTranslation()); + _messageShadowTranslation->translation.setValue(_GetMessageShadowTranslation()); + } +} + +double QtCoinViewer::_GetTextBaseSize(double size) +{ + if (size < 14.0) { + return 10.0; + } + if (size < 18.0) { + return 14.0; + } + if (size < 26.0) { + return 18.0; + } + return 26.0; +} + +SbVec3f QtCoinViewer::_GetMessageBaseTranslation() +{ + SbViewportRegion v = _pviewer->getViewportRegion(); + float fwratio = 964.0f/v.getWindowSize()[0], fhratio = 688.0f/v.getWindowSize()[1]; + float size = _messagefont->size.getValue(); + if (size < 14.0f) { + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.07f*fhratio),0); + } + if (size < 18.0f) { + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.098f*fhratio),0); + } + if (size < 26.0f) { + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.126f*fhratio),0); + } + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.182f*fhratio),0); +} + +SbVec3f QtCoinViewer::_GetMessageShadowTranslation() +{ + SbViewportRegion v = _pviewer->getViewportRegion(); + float fwratio = 964.0f/v.getWindowSize()[0], fhratio = 688.0f/v.getWindowSize()[1]; + float size = _messagefont->size.getValue(); + + if (size < 14.0f) { + return SbVec3f(-0.002f*fwratio,(0.032f*fhratio)*(size/10.0f),0); + } + if (size < 18.0f) { + return SbVec3f(-0.002f*fwratio,(0.0448f*fhratio)*(size/14.0f),0); + } + if (size < 26.0f) { + return SbVec3f(-0.002f*fwratio,(0.0540f*fhratio)*(size/18.0f),0); } + return SbVec3f(-0.002f*fwratio,(0.0777f*fhratio)*(size/26.0f),0); } bool QtCoinViewer::LoadModel(const string& pfilename) @@ -2809,9 +2895,7 @@ void QtCoinViewer::AdvanceFrame(bool bForward) } // adjust the shadow text - SbViewportRegion v = _pviewer->getViewportRegion(); - float fwratio = 964.0f/v.getWindowSize()[0], fhratio = 688.0f/v.getWindowSize()[1]; - _messageShadowTranslation->translation.setValue(SbVec3f(-0.002f*fwratio,0.032f*fhratio,0)); + _messageShadowTranslation->translation.setValue(_GetMessageShadowTranslation()); // search for all new lines string msg = ss.str(); @@ -3023,9 +3107,7 @@ void QtCoinViewer::UpdateFromModel() } if( pbody->IsRobot() ) { - boost::shared_ptr probot = boost::shared_ptr(new RobotItem(shared_viewer(), RaveInterfaceCast(pbody), _viewGeometryMode),ITEM_DELETER); - probot->SetTextSize(_textSize); - pitem = probot; + pitem = boost::shared_ptr(new RobotItem(shared_viewer(), RaveInterfaceCast(pbody), _viewGeometryMode),ITEM_DELETER); } else { pitem = boost::shared_ptr(new KinBodyItem(shared_viewer(), pbody, _viewGeometryMode),ITEM_DELETER); diff --git a/plugins/qtcoinrave/qtcoinviewer.h b/plugins/qtcoinrave/qtcoinviewer.h index b3978b55eb..9305a8fcf9 100644 --- a/plugins/qtcoinrave/qtcoinviewer.h +++ b/plugins/qtcoinrave/qtcoinviewer.h @@ -341,6 +341,10 @@ public slots: virtual void _SetTriangleMesh(SoSeparator* pparent, const float* ppoints, int stride, const int* pIndices, int numTriangles); virtual void _Reset(); virtual void _SetBkgndColor(const RaveVector& color); + virtual void _SetTextSize(double size); + virtual double _GetTextBaseSize(double size); + virtual SbVec3f _GetMessageBaseTranslation(); + virtual SbVec3f _GetMessageShadowTranslation(); virtual void _closegraph(SoSwitch* handle); virtual void _SetGraphTransform(SoSwitch* handle, const RaveTransform& t); virtual void _SetGraphShow(SoSwitch* handle, bool bshow); @@ -413,7 +417,9 @@ public slots: std::list< boost::shared_ptr > _plistdraggers; /// draggers drawn SoEventCallback* _eventKeyboardCB; + SoFont* _messagefont; boost::array _messageNodes; + SoTranslation* _messageBaseTranslation; SoTranslation* _messageShadowTranslation; bool _altDown[2]; @@ -422,7 +428,6 @@ public slots: std::string _name; std::string _userText; - double _textSize; std::map _mapbodies; ///< all the bodies created ItemPtr _pSelectedItem; ///< the currently selected item @@ -528,6 +533,7 @@ public slots: friend class DeselectMessage; friend class ResetMessage; friend class SetBkgndColorMessage; + friend class SetTextSizeMessage; friend class StartPlaybackTimerMessage; friend class StopPlaybackTimerMessage; friend class SetGraphTransformMessage; diff --git a/plugins/qtosgrave/osgviewerwidget.cpp b/plugins/qtosgrave/osgviewerwidget.cpp index def827498d..6a0982cdc2 100644 --- a/plugins/qtosgrave/osgviewerwidget.cpp +++ b/plugins/qtosgrave/osgviewerwidget.cpp @@ -1170,7 +1170,7 @@ void QOSGViewerWidget::SetViewport(int width, int height) void QOSGViewerWidget::SetHUDTextSize(double size) { - if ( size >= 0 ) { + if ( size > 0 ) { _hudTextSize = size; float scale = this->devicePixelRatio(); From 589a70474b011faac81d2f73a8dc0643a361f01e Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 14 Mar 2024 09:39:14 +0900 Subject: [PATCH 05/14] Fix qtcoinrave textSize implementation to target message nodes, add breakpoints for textSize and message translations --- plugins/qtcoinrave/qtcoinviewer.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/plugins/qtcoinrave/qtcoinviewer.cpp b/plugins/qtcoinrave/qtcoinviewer.cpp index 1fabe908d1..ef7467237c 100644 --- a/plugins/qtcoinrave/qtcoinviewer.cpp +++ b/plugins/qtcoinrave/qtcoinviewer.cpp @@ -259,7 +259,7 @@ void QtCoinViewer::_InitConstructor(std::istream& sinput) SoSeparator* pmsgsep = new SoSeparator(); _messagefont = new SoFont(); - _messagefont->size = 18; + _messagefont->size = 10; pmsgsep->addChild(_messagefont); _messageBaseTranslation = new SoTranslation(); @@ -704,16 +704,18 @@ SbVec3f QtCoinViewer::_GetMessageBaseTranslation() SbViewportRegion v = _pviewer->getViewportRegion(); float fwratio = 964.0f/v.getWindowSize()[0], fhratio = 688.0f/v.getWindowSize()[1]; float size = _messagefont->size.getValue(); + // magic window-size-independent constants that ensure that all preset text sizes + // are roughly vertically aligned by the top of the first line if (size < 14.0f) { return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.07f*fhratio),0); } if (size < 18.0f) { - return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.098f*fhratio),0); + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.082f*fhratio),0); } if (size < 26.0f) { - return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.126f*fhratio),0); + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.106f*fhratio),0); } - return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.182f*fhratio),0); + return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.126f*fhratio),0); } SbVec3f QtCoinViewer::_GetMessageShadowTranslation() @@ -721,7 +723,8 @@ SbVec3f QtCoinViewer::_GetMessageShadowTranslation() SbViewportRegion v = _pviewer->getViewportRegion(); float fwratio = 964.0f/v.getWindowSize()[0], fhratio = 688.0f/v.getWindowSize()[1]; float size = _messagefont->size.getValue(); - + // magic window-size-independent constants that ensure that the message shadow + // sits roughly one pixel above the base message text if (size < 14.0f) { return SbVec3f(-0.002f*fwratio,(0.032f*fhratio)*(size/10.0f),0); } @@ -2894,7 +2897,8 @@ void QtCoinViewer::AdvanceFrame(bool bForward) ss << _userText << endl; } - // adjust the shadow text + // adjust the text offsets + _messageBaseTranslation->translation.setValue(_GetMessageBaseTranslation()); _messageShadowTranslation->translation.setValue(_GetMessageShadowTranslation()); // search for all new lines From 3207fc279b2bc9033a0d2a959aceb5fde1f62642 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Fri, 15 Mar 2024 10:23:44 +0900 Subject: [PATCH 06/14] restore removed newline in qtcoinrave item.cpp item.h --- plugins/qtcoinrave/item.cpp | 2 +- plugins/qtcoinrave/item.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/plugins/qtcoinrave/item.cpp b/plugins/qtcoinrave/item.cpp index 06bdb05254..d8030d5f08 100644 --- a/plugins/qtcoinrave/item.cpp +++ b/plugins/qtcoinrave/item.cpp @@ -768,4 +768,4 @@ bool RobotItem::UpdateFromModel(const vector& vjointvalues, const vector< } return true; -} \ No newline at end of file +} diff --git a/plugins/qtcoinrave/item.h b/plugins/qtcoinrave/item.h index bee07117f7..7e8806eb6d 100644 --- a/plugins/qtcoinrave/item.h +++ b/plugins/qtcoinrave/item.h @@ -248,4 +248,4 @@ BOOST_TYPEOF_REGISTER_TYPE(RobotItem) BOOST_TYPEOF_REGISTER_TYPE(RobotItem::EE) #endif -#endif // ITEM_H \ No newline at end of file +#endif // ITEM_H From 1a13ecb5d2b3d68486922d4c27fb129177e064a7 Mon Sep 17 00:00:00 2001 From: Jonathan Sun Date: Wed, 20 Mar 2024 12:02:24 +0900 Subject: [PATCH 07/14] Update plugins/qtcoinrave/qtcoinviewer.h rename _messagefont -> _messageFont Co-authored-by: Binbin --- plugins/qtcoinrave/qtcoinviewer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/plugins/qtcoinrave/qtcoinviewer.h b/plugins/qtcoinrave/qtcoinviewer.h index 9305a8fcf9..9e3296569f 100644 --- a/plugins/qtcoinrave/qtcoinviewer.h +++ b/plugins/qtcoinrave/qtcoinviewer.h @@ -417,7 +417,7 @@ public slots: std::list< boost::shared_ptr > _plistdraggers; /// draggers drawn SoEventCallback* _eventKeyboardCB; - SoFont* _messagefont; + SoFont* _messageFont; boost::array _messageNodes; SoTranslation* _messageBaseTranslation; SoTranslation* _messageShadowTranslation; From 7b3ddacafc97ef80171918aa4ecc5d5e21e6c601 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Wed, 20 Mar 2024 14:12:13 +0900 Subject: [PATCH 08/14] _messagefont -> _messageFont in qtcoinviewer.cpp --- plugins/qtcoinrave/qtcoinviewer.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/plugins/qtcoinrave/qtcoinviewer.cpp b/plugins/qtcoinrave/qtcoinviewer.cpp index ef7467237c..4049da5b27 100644 --- a/plugins/qtcoinrave/qtcoinviewer.cpp +++ b/plugins/qtcoinrave/qtcoinviewer.cpp @@ -258,9 +258,9 @@ void QtCoinViewer::_InitConstructor(std::istream& sinput) // add the message texts SoSeparator* pmsgsep = new SoSeparator(); - _messagefont = new SoFont(); - _messagefont->size = 10; - pmsgsep->addChild(_messagefont); + _messageFont = new SoFont(); + _messageFont->size = 10; + pmsgsep->addChild(_messageFont); _messageBaseTranslation = new SoTranslation(); _messageBaseTranslation->translation.setValue(SbVec3f(-0.978f,0.874f,0)); @@ -678,7 +678,7 @@ void QtCoinViewer::_SetTextSize(double size) if ( size > 0 ) { // TODO: use a font that does not rely on hardcoded breakpoints // move down to next text size breakpoint so that message shadow aligns nicely - _messagefont->size = _GetTextBaseSize(size); + _messageFont->size = _GetTextBaseSize(size); // adjust the text offsets _messageBaseTranslation->translation.setValue(_GetMessageBaseTranslation()); _messageShadowTranslation->translation.setValue(_GetMessageShadowTranslation()); @@ -703,7 +703,7 @@ SbVec3f QtCoinViewer::_GetMessageBaseTranslation() { SbViewportRegion v = _pviewer->getViewportRegion(); float fwratio = 964.0f/v.getWindowSize()[0], fhratio = 688.0f/v.getWindowSize()[1]; - float size = _messagefont->size.getValue(); + float size = _messageFont->size.getValue(); // magic window-size-independent constants that ensure that all preset text sizes // are roughly vertically aligned by the top of the first line if (size < 14.0f) { @@ -722,7 +722,7 @@ SbVec3f QtCoinViewer::_GetMessageShadowTranslation() { SbViewportRegion v = _pviewer->getViewportRegion(); float fwratio = 964.0f/v.getWindowSize()[0], fhratio = 688.0f/v.getWindowSize()[1]; - float size = _messagefont->size.getValue(); + float size = _messageFont->size.getValue(); // magic window-size-independent constants that ensure that the message shadow // sits roughly one pixel above the base message text if (size < 14.0f) { From 9b1f9c6bb1d9ac48235c30039e18bf5634ace930 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Wed, 20 Mar 2024 14:59:35 +0900 Subject: [PATCH 09/14] add comments to text-size- and text-translation-related methods in qtcoinviewer.cpp add more detailed comments explaining what _GetTextBaseSize, _GetMessageBaseTranslation, and _GetMessageShadowTranslation do --- plugins/qtcoinrave/qtcoinviewer.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/plugins/qtcoinrave/qtcoinviewer.cpp b/plugins/qtcoinrave/qtcoinviewer.cpp index 4049da5b27..4bd2f32f28 100644 --- a/plugins/qtcoinrave/qtcoinviewer.cpp +++ b/plugins/qtcoinrave/qtcoinviewer.cpp @@ -685,6 +685,8 @@ void QtCoinViewer::_SetTextSize(double size) } } +// determines a text size that works best with the default qtcoin font (one of 10px, 14px, 18px, 26px) +// \param size requested text size double QtCoinViewer::_GetTextBaseSize(double size) { if (size < 14.0) { @@ -699,6 +701,9 @@ double QtCoinViewer::_GetTextBaseSize(double size) return 26.0; } +// based on the current font size of HUD text, computes the 3D offset of the black message "base" +// node relative to the center of the text plane such that the message aligns nicely with +// the upper left corner across all preset text sizes SbVec3f QtCoinViewer::_GetMessageBaseTranslation() { SbViewportRegion v = _pviewer->getViewportRegion(); @@ -718,6 +723,10 @@ SbVec3f QtCoinViewer::_GetMessageBaseTranslation() return SbVec3f(-1.0f+(0.022f*fwratio),1.0f-(0.126f*fhratio),0); } +// based on the current font size of HUD text, computes the 3D offset of the white message "shadow" +// node relative to the message "base" node such that the shadow lies roughly one pixel leftward +// and upward of the base message, helping to create a shading effect that improves overall HUD text +// visibility SbVec3f QtCoinViewer::_GetMessageShadowTranslation() { SbViewportRegion v = _pviewer->getViewportRegion(); From f7a8847b4ebc8a2035e4276f0e62603ca1020a43 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Thu, 21 Mar 2024 08:59:56 +0900 Subject: [PATCH 10/14] add zero size support to viewer.SetTextSize --- plugins/qtcoinrave/qtcoinviewer.cpp | 10 ++++++++-- plugins/qtcoinrave/qtcoinviewer.h | 1 + plugins/qtosgrave/osgviewerwidget.cpp | 2 +- 3 files changed, 10 insertions(+), 3 deletions(-) diff --git a/plugins/qtcoinrave/qtcoinviewer.cpp b/plugins/qtcoinrave/qtcoinviewer.cpp index 4bd2f32f28..e3a569a812 100644 --- a/plugins/qtcoinrave/qtcoinviewer.cpp +++ b/plugins/qtcoinrave/qtcoinviewer.cpp @@ -280,7 +280,11 @@ void QtCoinViewer::_InitConstructor(std::istream& sinput) _messageNodes[1] = new SoText2(); pmsgsep->addChild(_messageNodes[1]); - _ivRoot->addChild(pmsgsep); + _messageSwitch = new SoSwitch(); + _messageSwitch->whichChild.setValue(SO_SWITCH_ALL); + _messageSwitch->addChild(pmsgsep); + + _ivRoot->addChild(_messageSwitch); _ivRoot->addChild(_ivCamera); SoEventCallback * ecb = new SoEventCallback; @@ -675,10 +679,12 @@ void QtCoinViewer::SetTextSize(double size) void QtCoinViewer::_SetTextSize(double size) { - if ( size > 0 ) { + if ( size >= 0 ) { // TODO: use a font that does not rely on hardcoded breakpoints // move down to next text size breakpoint so that message shadow aligns nicely _messageFont->size = _GetTextBaseSize(size); + // hide HUD text if requested size is 0 + _messageSwitch->whichChild.setValue(size == 0 ? SO_SWITCH_NONE : SO_SWITCH_ALL); // adjust the text offsets _messageBaseTranslation->translation.setValue(_GetMessageBaseTranslation()); _messageShadowTranslation->translation.setValue(_GetMessageShadowTranslation()); diff --git a/plugins/qtcoinrave/qtcoinviewer.h b/plugins/qtcoinrave/qtcoinviewer.h index 9e3296569f..064535ba9f 100644 --- a/plugins/qtcoinrave/qtcoinviewer.h +++ b/plugins/qtcoinrave/qtcoinviewer.h @@ -417,6 +417,7 @@ public slots: std::list< boost::shared_ptr > _plistdraggers; /// draggers drawn SoEventCallback* _eventKeyboardCB; + SoSwitch* _messageSwitch; SoFont* _messageFont; boost::array _messageNodes; SoTranslation* _messageBaseTranslation; diff --git a/plugins/qtosgrave/osgviewerwidget.cpp b/plugins/qtosgrave/osgviewerwidget.cpp index 6a0982cdc2..def827498d 100644 --- a/plugins/qtosgrave/osgviewerwidget.cpp +++ b/plugins/qtosgrave/osgviewerwidget.cpp @@ -1170,7 +1170,7 @@ void QOSGViewerWidget::SetViewport(int width, int height) void QOSGViewerWidget::SetHUDTextSize(double size) { - if ( size > 0 ) { + if ( size >= 0 ) { _hudTextSize = size; float scale = this->devicePixelRatio(); From 2aecde0aa1eb09c07f7b7c04eed7dd6c230164dd Mon Sep 17 00:00:00 2001 From: Puttichai Date: Fri, 29 Mar 2024 19:18:39 +0900 Subject: [PATCH 11/14] Adds missing check of _vtempconfig from the last iteration for IT_Default. The check needs to be done regardless of bCheckEnd or bHasRampDeviatedFromInterpolation. --- src/libopenrave/planningutils.cpp | 46 ++++++++++++++----------------- 1 file changed, 21 insertions(+), 25 deletions(-) diff --git a/src/libopenrave/planningutils.cpp b/src/libopenrave/planningutils.cpp index 00efc4680d..ca026fac00 100644 --- a/src/libopenrave/planningutils.cpp +++ b/src/libopenrave/planningutils.cpp @@ -3234,33 +3234,29 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } } - if( bCheckEnd && bHasNewTempConfigToAdd && bHasRampDeviatedFromInterpolation ) { - dReal dist = params->_distmetricfn(_vtempconfig, q1); + if( bHasNewTempConfigToAdd ) { + // At this point, _vtempconfig is expected to have reached q1. But if not, then need to check _vtempconfig. + const dReal dist = params->_distmetricfn(_vtempconfig, q1); if( dist > 1e-7 ) { - RAVELOG_DEBUG_FORMAT("env=%d, ramp has deviated, so most likely q1 is not following constraints and there's a difference dist=%f", _listCheckBodies.front()->GetEnv()->GetId()%dist); - bCheckEnd = false; // to prevent adding the last point - - if( !!filterreturn ) { - if( options & CFO_FillCheckedConfiguration ) { - int nstateret = 0; - if( istep >= start ) { - nstateret = _SetAndCheckState(params, _vtempconfig, _vtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn); - bHasNewTempConfigToAdd = false; - if( !!params->_getstatefn ) { - params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits - } - if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) { - filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end()); - filterreturn->_configurationtimes.push_back(timestep); - } - } - if( nstateret != 0 ) { - if( !!filterreturn ) { - filterreturn->_returncode = nstateret; - } - return nstateret; - } + bCheckEnd = false; // _vtempconfig ends up far from q1 and we haven't checked the segment connecting _vtempconfig and q1 so prevent adding q1 to the list of checked configuration to tell the caller that the checked segment ends here. + int nstateret = 0; + nstateret = _SetAndCheckState(params, _vtempconfig, _vtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn); + bHasNewTempConfigToAdd = false; + if( !!params->_getstatefn ) { + params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits + } + if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) { + filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end()); + filterreturn->_configurationtimes.push_back(timestep); + } + if( nstateret != 0 ) { + if( !!filterreturn ) { + filterreturn->_returncode = nstateret; + filterreturn->_invalidvalues = _vtempconfig; + filterreturn->_invalidvelocities = _vtempvelconfig; + filterreturn->_fTimeWhenInvalid = timestep; } + return nstateret; } } } From ceb1092bcd1b3da3ab7416e559c284445ed0d211 Mon Sep 17 00:00:00 2001 From: Puttichai Date: Fri, 29 Mar 2024 19:23:37 +0900 Subject: [PATCH 12/14] Adds fixes for the remaining case - If the given velocities are valid (which they might in case IT_AllLinear), then compute the correct velocities to use in _SetAndCheckState. - Adds missing checking of the the last computed configuration _vtempconfig. --- src/libopenrave/planningutils.cpp | 87 ++++++++++++++++++------------- 1 file changed, 52 insertions(+), 35 deletions(-) diff --git a/src/libopenrave/planningutils.cpp b/src/libopenrave/planningutils.cpp index ca026fac00..5f434fb1b6 100644 --- a/src/libopenrave/planningutils.cpp +++ b/src/libopenrave/planningutils.cpp @@ -3273,13 +3273,7 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: *it *= fisteps; } - // just in case, have to set the current values to _vtempconfig since neighstatefn expects the state to be set. - if( params->SetStateValues(_vtempconfig, 0) != 0 ) { - if( !!filterreturn ) { - filterreturn->_returncode = CFO_StateSettingError; - } - return CFO_StateSettingError; - } + const bool validVelocities = (timeelapsed > 0) && (dq0.size() == _vtempconfig.size()) && (dq1.size() == _vtempconfig.size()); _vdiffconfig.resize(dQ.size()); _vstepconfig.resize(dQ.size()); @@ -3362,8 +3356,12 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } } // end checking configurations between _vtempconfig2 (the previous _vtempconfig) and _vtempconfig (the new one) } // end if maxnumsteps > 1 + } // end check neighstatus + if( validVelocities ) { + for( size_t idof = 0; idof < dQ.size(); ++idof ) { + _vtempvelconfig.at(idof) = dq0.at(idof) + _vtempveldelta.at(idof); + } } - // Else, _neighstatefn returns _vtempconfig + dQ. } _vprevtempconfig.resize(dQ.size()); @@ -3481,11 +3479,39 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } } // end collision checking } // end if maxnumsteps > 1 + } // end check neighstatus + if( validVelocities ) { + for( size_t idof = 0; idof < dQ.size(); ++idof ) { + _vtempvelconfig.at(idof) = dq0.at(idof) + _vtempveldelta.at(idof); + } + } + } // end for + + // At this point, _vtempconfig is not checked yet! + const dReal dist = params->_distmetricfn(_vtempconfig, q1); + if( dist > 1e-7 ) { + // _vtempconfig is different from q1 so must check it. + int nstateret = 0; + nstateret = _SetAndCheckState(params, _vtempconfig, _vtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn); + if( !!params->_getstatefn ) { + params->_getstatefn(_vtempconfig); // query again in order to get normalizations/joint limits + } + if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) { + filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end()); + filterreturn->_configurationtimes.push_back(1.0); + } + if( nstateret != 0 ) { + if( !!filterreturn ) { + filterreturn->_returncode = nstateret; + filterreturn->_invalidvalues = _vtempconfig; + if( validVelocities ) { + filterreturn->_invalidvelocities = _vtempvelconfig; + } + filterreturn->_fTimeWhenInvalid = 1.0; + } + return nstateret; } - } - // check if _vtempconfig is close to q1! - { // the neighbor function could be a constraint function and might move _vtempconfig by more than the specified dQ! so double check the straight light distance between them justin case? // TODO check if acceleration limits are satisfied between _vtempconfig, _vprevtempconfig, and _vprevtempvelconfig int numPostNeighSteps = 1; @@ -3501,14 +3527,8 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } if( numPostNeighSteps > 1 ) { - bHasRampDeviatedFromInterpolation = true; - // should never happen, but just in case _neighstatefn is some non-linear constraint projection - if( _listCheckBodies.size() > 0 ) { - RAVELOG_WARN_FORMAT("env=%d, have to divide the arc in %d steps even after original interpolation is done, interval=%d", _listCheckBodies.front()->GetEnv()->GetId()%numPostNeighSteps%interval); - } - else { - RAVELOG_WARN_FORMAT("have to divide the arc in %d steps even after original interpolation is done, interval=%d", numPostNeighSteps%interval); - } + bHasRampDeviatedFromInterpolation = true; // set here again just in case + RAVELOG_WARN_FORMAT("env=%s, have to divide the arc in %d steps even after original interpolation is done, interval=%d", _listCheckBodies.front()->GetEnv()->GetNameId()%numPostNeighSteps%interval); // this case should be rare, so can create a vector here. don't look at constraints since we would never converge... // note that circular constraints would break here @@ -3524,7 +3544,7 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: _vprevtempconfig = _vtempconfig; _vprevtempvelconfig = _vtempvelconfig; // do only numPostNeighSteps-1 since the last step should be checked by _vtempconfig - for(int ipoststep = 0; ipoststep+1 < numPostNeighSteps; ++ipoststep) { + for(int ipoststep = 0; ipoststep < numPostNeighSteps; ++ipoststep) { for( int idof = 0; idof < (int)_vtempconfig.size(); ++idof) { _vprevtempconfig[idof] += vpostdq[idof]; } @@ -3534,24 +3554,21 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } } - int nstateret = _SetAndCheckState(params, _vprevtempconfig, _vprevtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn); -// if( !!params->_getstatefn ) { -// params->_getstatefn(_vprevtempconfig); // query again in order to get normalizations/joint limits -// } - // since the timeelapsed is not clear, it is dangerous to write filterreturn->_configurations and filterreturn->_configurationtimes since it could force programing using those times to accelerate too fast. so don't write -// if( !!filterreturn && (options & CFO_FillCheckedConfiguration) ) { -// filterreturn->_configurations.insert(filterreturn->_configurations.end(), _vtempconfig.begin(), _vtempconfig.end()); -// filterreturn->_configurationtimes.push_back(timestep); -// } - if( nstateret != 0 ) { + int npostneighret = _SetAndCheckState(params, _vprevtempconfig, _vprevtempvelconfig, _vtempaccelconfig, maskoptions, filterreturn); + if( npostneighret != 0 ) { if( !!filterreturn ) { - filterreturn->_returncode = nstateret; + filterreturn->_returncode = npostneighret; + filterreturn->_invalidvalues = _vprevtempconfig; + if( validVelocities ) { + filterreturn->_invalidvelocities = _vprevtempvelconfig; + } + filterreturn->_fTimeWhenInvalid = 1.0; } - return nstateret; + return npostneighret; } - } - } - } + } // end for ipoststep + } // end numPostNeighSteps > 1 + } // end dist > 1e-7 } if( !!filterreturn ) { From a7cb1ab811b8d71f1486e1435d9d622b99b4666d Mon Sep 17 00:00:00 2001 From: Puttichai Date: Tue, 2 Apr 2024 13:16:36 +0900 Subject: [PATCH 13/14] Updates changelog and bumps version --- CMakeLists.txt | 2 +- docs/source/changelog.rst | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c95d70540..9633e00aa2 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 141) -set (OPENRAVE_VERSION_PATCH 1) +set (OPENRAVE_VERSION_PATCH 2) 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 08786650dc..87d0ef6877 100644 --- a/docs/source/changelog.rst +++ b/docs/source/changelog.rst @@ -3,6 +3,11 @@ ChangeLog ######### +Version 0.141.2 +=============== + +* Fix the issue that second-to-last configuration along the given path segment may not be checked in `Check` function. + Version 0.141.1 =============== From 7f059acc945d73d0f4cc00f87d82b7ab129ef5bc Mon Sep 17 00:00:00 2001 From: Puttichai Date: Tue, 2 Apr 2024 15:25:23 +0900 Subject: [PATCH 14/14] Fixes velocity computation --- src/libopenrave/planningutils.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/libopenrave/planningutils.cpp b/src/libopenrave/planningutils.cpp index 5f434fb1b6..64099b1065 100644 --- a/src/libopenrave/planningutils.cpp +++ b/src/libopenrave/planningutils.cpp @@ -3358,6 +3358,7 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } // end if maxnumsteps > 1 } // end check neighstatus if( validVelocities ) { + // Compute the next velocity for( size_t idof = 0; idof < dQ.size(); ++idof ) { _vtempvelconfig.at(idof) = dq0.at(idof) + _vtempveldelta.at(idof); } @@ -3481,8 +3482,9 @@ int DynamicsCollisionConstraint::Check(const std::vector& q0, const std:: } // end if maxnumsteps > 1 } // end check neighstatus if( validVelocities ) { + // Compute the next velocity for( size_t idof = 0; idof < dQ.size(); ++idof ) { - _vtempvelconfig.at(idof) = dq0.at(idof) + _vtempveldelta.at(idof); + _vtempvelconfig.at(idof) = dq0.at(idof) + dReal(f + 1)*_vtempveldelta.at(idof); } } } // end for