Merge branch 'production' into rs-cache-grabber-state-saver

3rdparty/ann/src/ANN.cpp

@@ -46,9 +46,9 @@ ANNdist annDist(						// interpoint squared distance
 	ANNpoint			p,
 	ANNpoint			q)
 {
-	register int d;
-	register ANNcoord diff;
-	register ANNcoord dist;
+	int d;
+	ANNcoord diff;
+	ANNcoord dist;
 
 	dist = 0;
 	for (d = 0; d < dim; d++) {

3rdparty/ann/src/kd_fix_rad_search.cpp

@@ -147,11 +147,11 @@ void ANNkd_split::ann_FR_search(ANNdist box_dist)
 
 void ANNkd_leaf::ann_FR_search(ANNdist box_dist)
 {
-	register ANNdist dist;				// distance to data point
-	register ANNcoord* pp;				// data coordinate pointer
-	register ANNcoord* qq;				// query coordinate pointer
-	register ANNcoord t;
-	register int d;
+	ANNdist dist;				// distance to data point
+	ANNcoord* pp;				// data coordinate pointer
+	ANNcoord* qq;				// query coordinate pointer
+	ANNcoord t;
+	int d;
 
 	for (int i = 0; i < n_pts; i++) {	// check points in bucket
 

3rdparty/ann/src/kd_pr_search.cpp

@@ -180,12 +180,12 @@ void ANNkd_split::ann_pri_search(ANNdist box_dist)
 
 void ANNkd_leaf::ann_pri_search(ANNdist box_dist)
 {
-	register ANNdist dist;				// distance to data point
-	register ANNcoord* pp;				// data coordinate pointer
-	register ANNcoord* qq;				// query coordinate pointer
-	register ANNdist min_dist;			// distance to k-th closest point
-	register ANNcoord t;
-	register int d;
+	ANNdist dist;				// distance to data point
+	ANNcoord* pp;				// data coordinate pointer
+	ANNcoord* qq;				// query coordinate pointer
+	ANNdist min_dist;			// distance to k-th closest point
+	ANNcoord t;
+	int d;
 
 	min_dist = ANNprPointMK->max_key(); // k-th smallest distance so far
 

3rdparty/ann/src/kd_search.cpp

@@ -171,12 +171,12 @@ void ANNkd_split::ann_search(ANNpoint& ANNkdQ, ANNmin_k* ANNkdPointMK, ANNdist b
 
 void ANNkd_leaf::ann_search(ANNpoint& ANNkdQ, ANNmin_k* ANNkdPointMK, ANNdist box_dist)
 {
-	register ANNdist dist;				// distance to data point
-	register ANNcoord* pp;				// data coordinate pointer
-	register ANNcoord* qq;				// query coordinate pointer
-	register ANNdist min_dist;			// distance to k-th closest point
-	register ANNcoord t;
-	register int d;
+	ANNdist dist;				// distance to data point
+	ANNcoord* pp;				// data coordinate pointer
+	ANNcoord* qq;				// query coordinate pointer
+	ANNdist min_dist;			// distance to k-th closest point
+	ANNcoord t;
+	int d;
 
 	min_dist = ANNkdPointMK->max_key(); // k-th smallest distance so far
 

3rdparty/ann/src/kd_util.cpp

@@ -127,10 +127,10 @@ ANNdist annBoxDistance(			// compute distance from point to box
 	const ANNpoint		hi,				// high point of box
 	int					dim)			// dimension of space
 {
-	register ANNdist dist = 0.0;		// sum of squared distances
-	register ANNdist t;
+	ANNdist dist = 0.0;		// sum of squared distances
+	ANNdist t;
 
-	for (register int d = 0; d < dim; d++) {
+	for (int d = 0; d < dim; d++) {
 		if (q[d] < lo[d]) {				// q is left of box
 			t = ANNdist(lo[d]) - ANNdist(q[d]);
 			dist = ANN_SUM(dist, ANN_POW(t));
@@ -238,8 +238,8 @@ void annMedianSplit(
 	int l = 0;							// left end of current subarray
 	int r = n-1;						// right end of current subarray
 	while (l < r) {
-		register int i = (r+l)/2;		// select middle as pivot
-		register int k;
+		int i = (r+l)/2;		// select middle as pivot
+		int k;
 
 		if (PA(i,d) > PA(r,d))			// make sure last > pivot
 			PASWAP(i,r)

3rdparty/ann/src/pr_queue.h

@@ -86,9 +86,9 @@ class ANNpr_queue {
 		PQinfo inf)						// item info
 		{
 			if (++n > max_size) annError("Priority queue overflow.", ANNabort);
-			register int r = n;
+			int r = n;
 			while (r > 1) {				// sift up new item
-				register int p = r/2;
+				int p = r/2;
 				ANN_FLOP(1)				// increment floating ops
 				if (pq[p].key <= kv)	// in proper order
 					break;
@@ -105,9 +105,9 @@ class ANNpr_queue {
 		{
 			kv = pq[1].key;				// key of min item
 			inf = pq[1].info;			// information of min item
-			register PQkey kn = pq[n--].key;// last item in queue
-			register int p = 1;			// p points to item out of position
-			register int r = p<<1;		// left child of p
+			PQkey kn = pq[n--].key;// last item in queue
+			int p = 1;			// p points to item out of position
+			int r = p<<1;		// left child of p
 			while (r <= n) {			// while r is still within the heap
 				ANN_FLOP(2)				// increment floating ops
 										// set r to smaller child of p

3rdparty/ann/src/pr_queue_k.h

@@ -100,7 +100,7 @@ class ANNmin_k {
 		PQKkey kv,						// key value
 		PQKinfo inf)					// item info
 		{
-			register int i;
+			int i;
 										// slide larger values up
 			for (i = n; i > 0; i--) {
 				if (mk[i-1].key > kv)

CMakeLists.txt

@@ -4,7 +4,7 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 
 # Define here the needed parameters
 set (OPENRAVE_VERSION_MAJOR 0)
-set (OPENRAVE_VERSION_MINOR 159)
+set (OPENRAVE_VERSION_MINOR 160)
 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})
@@ -114,22 +114,6 @@ include(GNUInstallDirs)
 
 find_package(PkgConfig) # pkg_check_modules
 
-check_cxx_source_compiles(" 
-#include <mutex> 
-int main() 
-{ 
-    std::scoped_lock lock;
-    return 0;
-}"
-OPENRAVE_STD_SCOPED_LOCK
-)
-# need to convert to int to set config.h
-if(OPENRAVE_STD_SCOPED_LOCK)
-  set(OPENRAVE_STD_SCOPED_LOCK 1)
-else()
-  set(OPENRAVE_STD_SCOPED_LOCK 0)
-endif()
-
 check_cxx_source_compiles(" 
 #include <string_view> 
 int main() 
@@ -139,6 +123,7 @@ int main()
 }"
 OPENRAVE_STD_STRING_VIEW
 )
+# need to convert to int to set config.h
 if(OPENRAVE_STD_STRING_VIEW)
   set(OPENRAVE_STD_STRING_VIEW 1)
 else()

config.h.in

@@ -71,7 +71,6 @@
 #define OPENRAVE_CURL @OPENRAVE_CURL@
 
 #define OPENRAVE_ENVIRONMENT_RECURSIVE_LOCK @OPENRAVE_ENVIRONMENT_RECURSIVE_LOCK@
-#define OPENRAVE_STD_SCOPED_LOCK @OPENRAVE_STD_SCOPED_LOCK@
 #define OPENRAVE_STD_STRING_VIEW @OPENRAVE_STD_STRING_VIEW@
 
 #endif

docs/source/changelog.rst

@@ -3,6 +3,19 @@
 ChangeLog
 #########
 
+Version 0.160.0
+===============
+
+- Add new functions for AddKinBody/AddRobot to specify an exact environmentBodyIndex.
+- Use std::unique_lock instead of std::scoped_lock. Remove boost recursive mutex.
+
+Version 0.159.1
+===============
+
+- Fix the problem that connected body resolved joint names that are empty are not skipped in `_UpdateConnectedBodyInfo`.
+- Fix the problem that `CompareTransform` does not consider the quaternions `quat` and `-quat` to be the same rotation.
+- Fix wrong ComputeInverseDynamics for Prismatic joint.
+
 Version 0.159.0
 ===============
 

include/openrave/environment.h

@@ -23,23 +23,16 @@
 #define  OPENRAVE_ENVIRONMENTBASE_H
 
 #include <openrave/config.h>
+#include <mutex>
 
 namespace OpenRAVE {
 
 #if OPENRAVE_ENVIRONMENT_RECURSIVE_LOCK
-#if OPENRAVE_STD_SCOPED_LOCK
-#include <mutex>
 using EnvironmentMutex = ::std::recursive_mutex;
-using EnvironmentLock  = ::std::scoped_lock<std::recursive_mutex>;
+using EnvironmentLock  = ::std::unique_lock<std::recursive_mutex>;
 using defer_lock_t     = ::std::defer_lock_t;
 using try_to_lock_t    = ::std::try_to_lock_t;
 #else
-using EnvironmentMutex = ::boost::recursive_try_mutex;
-using EnvironmentLock  = EnvironmentMutex::scoped_lock;
-using defer_lock_t     = ::boost::defer_lock_t;
-using try_to_lock_t    = ::boost::try_to_lock_t;
-#endif // OPENRAVE_STD_SCOPED_LOCK
-#else
 using EnvironmentMutex = ::std::mutex;
 using EnvironmentLock  = ::std::unique_lock<std::mutex>;
 using defer_lock_t     = ::std::defer_lock_t;
@@ -482,6 +475,22 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this<Envir
 
     virtual void Add(InterfaceBasePtr pinterface, bool bAnonymous, const std::string& cmdargs=std::string()) RAVE_DEPRECATED;
 
+    /** \brief Add an body to the environment
+        \param[in] pbody the pointer to an initialized body
+        \param[in] addMode One of IAM_X
+        \param[in] requestedEnvironmentBodyIndex if positive and none of existing body uses it, this is assigned to pbody. If positive and existing body uses it, exception is thrown. If non-positive, environment body index is decided internally.
+        \throw openrave_exception Throw if interface is invalid or already added
+     */
+    virtual void AddKinBody(KinBodyPtr pbody, InterfaceAddMode addMode, int requestedEnvironmentBodyIndex) = 0;
+
+    /** \brief Add an robot to the environment
+        \param[in] probot the pointer to an initialized robot
+        \param[in] addMode One of IAM_X
+        \param[in] requestedEnvironmentBodyIndex if positive and none of existing body uses it, this is assigned to probot. If positive and existing body uses it, exception is thrown. If non-positive, environment body index is decided internally.
+        \throw openrave_exception Throw if interface is invalid or already added
+     */
+    virtual void AddRobot(RobotBasePtr probot, InterfaceAddMode addMode, int requestedEnvironmentBodyIndex) = 0;
+
     /// \brief bodycallback(body, action)
     ///
     /// \param body KinBodyPtr

include/openrave/geometry.h

@@ -493,8 +493,11 @@ class RaveTransform
 
     /// \brief return true if any element of transform is different by mor ethan epsilon, otherwise false.
     inline bool CompareTransform(const RaveTransform<T>& rhs, T epsilon) const {
-        return RaveFabs(trans.x - rhs.trans.x) > epsilon || RaveFabs(trans.y - rhs.trans.y) > epsilon || RaveFabs(trans.z - rhs.trans.z) > epsilon ||
-               RaveFabs(rot.x - rhs.rot.x) > epsilon || RaveFabs(rot.y - rhs.rot.y) > epsilon || RaveFabs(rot.z - rhs.rot.z) > epsilon || RaveFabs(rot.w - rhs.rot.w) > epsilon;
+        return (RaveFabs(trans.x - rhs.trans.x) > epsilon
+		|| RaveFabs(trans.y - rhs.trans.y) > epsilon
+		|| RaveFabs(trans.z - rhs.trans.z) > epsilon
+		|| ((RaveFabs(rot.x - rhs.rot.x) > epsilon || RaveFabs(rot.y - rhs.rot.y) > epsilon || RaveFabs(rot.z - rhs.rot.z) > epsilon || RaveFabs(rot.w - rhs.rot.w) > epsilon)
+		    && ((RaveFabs(rot.x + rhs.rot.x) > epsilon || RaveFabs(rot.y + rhs.rot.y) > epsilon || RaveFabs(rot.z + rhs.rot.z) > epsilon || RaveFabs(rot.w + rhs.rot.w) > epsilon))));
     }
 
     inline RaveTransform<T> inverse() const {

include/openrave/openraveexception.h

@@ -50,6 +50,7 @@ enum OpenRAVEErrorCode
     ORE_TimeDurationUnitInvalid = 18, ///< Cannot find the specific TimeDurationUnit
     ORE_AngleUnitInvalid = 19, ///< Cannot find the specific AngleUnit
     ORE_TimeStampUnitInvalid = 20, ///< Cannot find the specific TimeStampUnit
+    ORE_EnvironmentBodyIndexConflict=21, ///< body with same environment body index is trying to be added to the environment
 
     ORE_EnvironmentFormatUnrecognized = 0x0100, ///< the environment format to load is not recognized.
 

python/bindings/include/openravepy/openravepy_environmentbase.h

@@ -217,8 +217,10 @@ class OPENRAVEPY_API PyEnvironmentBase : public OPENRAVE_ENABLE_SHARED_FROM_THIS
 
     void AddKinBody(PyKinBodyPtr pbody);
     void AddKinBody(PyKinBodyPtr pbody, bool bAnonymous);
+    void AddKinBody(PyKinBodyPtr pbody, py::object oAddMode, int requestedEnvironmentBodyIndex);
     void AddRobot(PyRobotBasePtr robot);
     void AddRobot(PyRobotBasePtr robot, bool bAnonymous);
+    void AddRobot(PyRobotBasePtr robot, py::object oAddMode, int requestedEnvironmentBodyIndex);
     void AddSensor(PySensorBasePtr sensor);
     void AddSensor(PySensorBasePtr sensor, bool bAnonymous);
     void AddViewer(PyViewerBasePtr viewer);

python/bindings/openravepy_int.cpp

@@ -2156,20 +2156,28 @@ object PyEnvironmentBase::ReadTrimeshData(const std::string& data, const std::st
     return toPyTriMesh(*ptrimesh);
 }
 
-void PyEnvironmentBase::Add(PyInterfaceBasePtr pinterface, py::object oAddMode, const std::string& cmdargs)
+
+InterfaceAddMode _ExtractAddMode(const py::object oAddMode, PyInterfaceBasePtr pinterface, const EnvironmentBasePtr& penv)
 {
     InterfaceAddMode addMode = IAM_StrictNameChecking;
     if( !IS_PYTHONOBJECT_NONE(oAddMode) ) {
         if (PyBool_Check(oAddMode.ptr())) {
             addMode = py::extract<bool>(oAddMode) ? IAM_AllowRenaming : IAM_StrictNameChecking;
-            if( pinterface->GetInterfaceType() != PT_Module ) {
-                RAVELOG_WARN_FORMAT("env=%s trying to use 'anonymous' flag when adding object '%s' of interface type '%d' via Add", _penv->GetNameId()%pinterface->GetXMLId()%(int)pinterface->GetInterfaceType());
+            const InterfaceType type = pinterface->GetInterfaceType();
+            if( type != PT_Module && type != PT_Robot && type != PT_KinBody ) {
+                RAVELOG_WARN_FORMAT("env=%s trying to use 'anonymous' flag when adding object '%s' of interface type '%d' via Add", penv->GetNameId()%pinterface->GetXMLId()%(int)pinterface->GetInterfaceType());
             }
         }
         else {
             addMode = py::extract<InterfaceAddMode>(oAddMode);
         }
     }
+    return addMode;
+}
+
+void PyEnvironmentBase::Add(PyInterfaceBasePtr pinterface, py::object oAddMode, const std::string& cmdargs)
+{
+    const InterfaceAddMode addMode = _ExtractAddMode(oAddMode, pinterface, _penv);
     PythonThreadSaver threadsaver;
     _penv->Add(pinterface->GetInterfaceBase(), addMode, cmdargs);
 }
@@ -2181,6 +2189,10 @@ void PyEnvironmentBase::AddKinBody(PyKinBodyPtr pbody, bool bAnonymous) {
     RAVELOG_WARN("Calling AddKinBody with bAnonymous, should switch to IAM_X signals");
     CHECK_POINTER(pbody); _penv->Add(openravepy::GetKinBody(pbody),bAnonymous ? IAM_AllowRenaming : IAM_StrictNameChecking);
 }
+void PyEnvironmentBase::AddKinBody(PyKinBodyPtr pbody, py::object oAddMode, int requestedEnvironmentBodyIndex) {
+    CHECK_POINTER(pbody);
+    _penv->AddKinBody(openravepy::GetKinBody(pbody), _ExtractAddMode(oAddMode, pbody, _penv), requestedEnvironmentBodyIndex);
+}
 void PyEnvironmentBase::AddRobot(PyRobotBasePtr robot) {
     CHECK_POINTER(robot);
     _penv->Add(openravepy::GetRobot(robot), IAM_StrictNameChecking);
@@ -2190,6 +2202,10 @@ void PyEnvironmentBase::AddRobot(PyRobotBasePtr robot, bool bAnonymous) {
     CHECK_POINTER(robot);
     _penv->Add(openravepy::GetRobot(robot), bAnonymous ? IAM_AllowRenaming : IAM_StrictNameChecking);
 }
+void PyEnvironmentBase::AddRobot(PyRobotBasePtr robot, py::object oAddMode, int requestedEnvironmentBodyIndex) {
+    CHECK_POINTER(robot);
+    _penv->AddRobot(openravepy::GetRobot(robot), _ExtractAddMode(oAddMode, robot, _penv), requestedEnvironmentBodyIndex);
+}
 void PyEnvironmentBase::AddSensor(PySensorBasePtr sensor) {
     CHECK_POINTER(sensor);
     _penv->Add(openravepy::GetSensor(sensor), IAM_StrictNameChecking);
@@ -3662,8 +3678,10 @@ Because race conditions can pop up when trying to lock the openrave environment
 #endif
         void (PyEnvironmentBase::*addkinbody1)(PyKinBodyPtr) = &PyEnvironmentBase::AddKinBody;
         void (PyEnvironmentBase::*addkinbody2)(PyKinBodyPtr,bool) = &PyEnvironmentBase::AddKinBody;
+        void (PyEnvironmentBase::*addkinbody3)(PyKinBodyPtr,object,int) = &PyEnvironmentBase::AddKinBody;
         void (PyEnvironmentBase::*addrobot1)(PyRobotBasePtr) = &PyEnvironmentBase::AddRobot;
         void (PyEnvironmentBase::*addrobot2)(PyRobotBasePtr,bool) = &PyEnvironmentBase::AddRobot;
+        void (PyEnvironmentBase::*addrobot3)(PyRobotBasePtr,object,int) = &PyEnvironmentBase::AddRobot;
         void (PyEnvironmentBase::*addsensor1)(PySensorBasePtr) = &PyEnvironmentBase::AddSensor;
         void (PyEnvironmentBase::*addsensor2)(PySensorBasePtr,bool) = &PyEnvironmentBase::AddSensor;
         void (PyEnvironmentBase::*setuserdata1)(PyUserData) = &PyEnvironmentBase::SetUserData;
@@ -3835,8 +3853,10 @@ Because race conditions can pop up when trying to lock the openrave environment
 #endif
                      .def("AddKinBody",addkinbody1, PY_ARGS("body") DOXY_FN(EnvironmentBase,AddKinBody))
                      .def("AddKinBody",addkinbody2, PY_ARGS("body","anonymous") DOXY_FN(EnvironmentBase,AddKinBody))
+                     .def("AddKinBody",addkinbody3, PY_ARGS("body","addMode","requestedEnvironmentBodyIndex") DOXY_FN(EnvironmentBase,AddKinBody))
                      .def("AddRobot",addrobot1, PY_ARGS("robot") DOXY_FN(EnvironmentBase,AddRobot))
                      .def("AddRobot",addrobot2, PY_ARGS("robot","anonymous") DOXY_FN(EnvironmentBase,AddRobot))
+                     .def("AddRobot",addrobot3, PY_ARGS("robot","addMode","requestedEnvironmentBodyIndex") DOXY_FN(EnvironmentBase,AddRobot))
                      .def("AddSensor",addsensor1, PY_ARGS("sensor") DOXY_FN(EnvironmentBase,AddSensor))
                      .def("AddSensor",addsensor2, PY_ARGS("sensor","anonymous") DOXY_FN(EnvironmentBase,AddSensor))
                      .def("AddViewer",addsensor2, PY_ARGS("sensor","anonymous") DOXY_FN(EnvironmentBase,AddViewer))