From b69a286a799e7696e58052d03b0649e02f06e588 Mon Sep 17 00:00:00 2001 From: Das Date: Mon, 11 Mar 2024 16:55:45 +1000 Subject: [PATCH] updated to use variable time value for movement Updated for traditional move cases (joint pose, home, cartesian pose, named pose) Tested in simulation and needs verification on real robot --- armer/cython/collision_handler.cpp | 88 +++++++++++++---------------- armer/cython/collision_handler.html | 4 +- armer/robots/ROSRobot.py | 43 +++++++++++--- 3 files changed, 76 insertions(+), 59 deletions(-) diff --git a/armer/cython/collision_handler.cpp b/armer/cython/collision_handler.cpp index 0966e50..e2a1b67 100644 --- a/armer/cython/collision_handler.cpp +++ b/armer/cython/collision_handler.cpp @@ -1,4 +1,4 @@ -/* Generated by Cython 3.0.5 */ +/* Generated by Cython 3.0.9 */ /* BEGIN: Cython Metadata { @@ -38,10 +38,10 @@ END: Cython Metadata */ #else #define __PYX_EXTRA_ABI_MODULE_NAME "" #endif -#define CYTHON_ABI "3_0_5" __PYX_EXTRA_ABI_MODULE_NAME +#define CYTHON_ABI "3_0_9" __PYX_EXTRA_ABI_MODULE_NAME #define __PYX_ABI_MODULE_NAME "_cython_" CYTHON_ABI #define __PYX_TYPE_MODULE_PREFIX __PYX_ABI_MODULE_NAME "." -#define CYTHON_HEX_VERSION 0x030005F0 +#define CYTHON_HEX_VERSION 0x030009F0 #define CYTHON_FUTURE_DIVISION 1 #include #ifndef offsetof @@ -255,7 +255,7 @@ END: Cython Metadata */ #ifndef CYTHON_UPDATE_DESCRIPTOR_DOC #define CYTHON_UPDATE_DESCRIPTOR_DOC 0 #endif -#elif defined(PY_NOGIL) +#elif defined(Py_GIL_DISABLED) || defined(Py_NOGIL) #define CYTHON_COMPILING_IN_PYPY 0 #define CYTHON_COMPILING_IN_CPYTHON 0 #define CYTHON_COMPILING_IN_LIMITED_API 0 @@ -599,18 +599,19 @@ class __Pyx_FakeReference { PyObject *exception_table = NULL; PyObject *types_module=NULL, *code_type=NULL, *result=NULL; #if __PYX_LIMITED_VERSION_HEX < 0x030B0000 - PyObject *version_info; // borrowed - #endif + PyObject *version_info; PyObject *py_minor_version = NULL; + #endif long minor_version = 0; PyObject *type, *value, *traceback; PyErr_Fetch(&type, &value, &traceback); #if __PYX_LIMITED_VERSION_HEX >= 0x030B0000 - minor_version = 11; // we don't yet need to distinguish between versions > 11 + minor_version = 11; #else if (!(version_info = PySys_GetObject("version_info"))) goto end; if (!(py_minor_version = PySequence_GetItem(version_info, 1))) goto end; minor_version = PyLong_AsLong(py_minor_version); + Py_DECREF(py_minor_version); if (minor_version == -1 && PyErr_Occurred()) goto end; #endif if (!(types_module = PyImport_ImportModule("types"))) goto end; @@ -631,7 +632,6 @@ class __Pyx_FakeReference { Py_XDECREF(code_type); Py_XDECREF(exception_table); Py_XDECREF(types_module); - Py_XDECREF(py_minor_version); if (type) { PyErr_Restore(type, value, traceback); } @@ -664,7 +664,7 @@ class __Pyx_FakeReference { PyObject *fv, PyObject *cell, PyObject* fn, PyObject *name, int fline, PyObject *lnos) { PyCodeObject *result; - PyObject *empty_bytes = PyBytes_FromStringAndSize("", 0); // we don't have access to __pyx_empty_bytes here + PyObject *empty_bytes = PyBytes_FromStringAndSize("", 0); if (!empty_bytes) return NULL; result = #if PY_VERSION_HEX >= 0x030C0000 @@ -750,8 +750,13 @@ class __Pyx_FakeReference { typedef PyObject *(*__Pyx_PyCFunctionFastWithKeywords) (PyObject *self, PyObject *const *args, Py_ssize_t nargs, PyObject *kwnames); #else - #define __Pyx_PyCFunctionFast _PyCFunctionFast - #define __Pyx_PyCFunctionFastWithKeywords _PyCFunctionFastWithKeywords + #if PY_VERSION_HEX >= 0x030d00A4 + # define __Pyx_PyCFunctionFast PyCFunctionFast + # define __Pyx_PyCFunctionFastWithKeywords PyCFunctionFastWithKeywords + #else + # define __Pyx_PyCFunctionFast _PyCFunctionFast + # define __Pyx_PyCFunctionFastWithKeywords _PyCFunctionFastWithKeywords + #endif #endif #if CYTHON_METH_FASTCALL #define __Pyx_METH_FASTCALL METH_FASTCALL @@ -959,7 +964,7 @@ static CYTHON_INLINE PyObject * __Pyx_PyDict_GetItemStrWithError(PyObject *dict, #endif #if CYTHON_USE_TYPE_SPECS && PY_VERSION_HEX >= 0x03080000 #define __Pyx_PyHeapTypeObject_GC_Del(obj) {\ - PyTypeObject *type = Py_TYPE(obj);\ + PyTypeObject *type = Py_TYPE((PyObject*)obj);\ assert(__Pyx_PyType_HasFeature(type, Py_TPFLAGS_HEAPTYPE));\ PyObject_GC_Del(obj);\ Py_DECREF(type);\ @@ -1103,7 +1108,7 @@ static CYTHON_INLINE PyObject * __Pyx_PyDict_GetItemStrWithError(PyObject *dict, #define __Pyx_PyBytes_GET_SIZE(o) PyBytes_Size(o) #define __Pyx_PyByteArray_GET_SIZE(o) PyByteArray_Size(o) #endif -#if PY_VERSION_HEX >= 0x030d00A1 +#if __PYX_LIMITED_VERSION_HEX >= 0x030d00A1 #define __Pyx_PyImport_AddModuleRef(name) PyImport_AddModuleRef(name) #else static CYTHON_INLINE PyObject *__Pyx_PyImport_AddModuleRef(const char *name) { @@ -1190,7 +1195,7 @@ static CYTHON_INLINE float __PYX_NAN() { #endif #define __PYX_MARK_ERR_POS(f_index, lineno) \ - { __pyx_filename = __pyx_f[f_index]; (void)__pyx_filename; __pyx_lineno = lineno; (void)__pyx_lineno; __pyx_clineno = __LINE__; (void)__pyx_clineno; } + { __pyx_filename = __pyx_f[f_index]; (void)__pyx_filename; __pyx_lineno = lineno; (void)__pyx_lineno; __pyx_clineno = __LINE__; (void)__pyx_clineno; } #define __PYX_ERR(f_index, lineno, Ln_error) \ { __PYX_MARK_ERR_POS(f_index, lineno) goto Ln_error; } @@ -1298,24 +1303,7 @@ static CYTHON_INLINE PyObject* __Pyx_PyUnicode_FromString(const char*); #define __Pyx_PyByteArray_FromCString(s) __Pyx_PyByteArray_FromString((const char*)s) #define __Pyx_PyStr_FromCString(s) __Pyx_PyStr_FromString((const char*)s) #define __Pyx_PyUnicode_FromCString(s) __Pyx_PyUnicode_FromString((const char*)s) -#if CYTHON_COMPILING_IN_LIMITED_API -static CYTHON_INLINE size_t __Pyx_Py_UNICODE_strlen(const wchar_t *u) -{ - const wchar_t *u_end = u; - while (*u_end++) ; - return (size_t)(u_end - u - 1); -} -#else -static CYTHON_INLINE size_t __Pyx_Py_UNICODE_strlen(const Py_UNICODE *u) -{ - const Py_UNICODE *u_end = u; - while (*u_end++) ; - return (size_t)(u_end - u - 1); -} -#endif #define __Pyx_PyUnicode_FromOrdinal(o) PyUnicode_FromOrdinal((int)o) -#define __Pyx_PyUnicode_FromUnicode(u) PyUnicode_FromUnicode(u, __Pyx_Py_UNICODE_strlen(u)) -#define __Pyx_PyUnicode_FromUnicodeAndLength PyUnicode_FromUnicode #define __Pyx_PyUnicode_AsUnicode PyUnicode_AsUnicode #define __Pyx_NewRef(obj) (Py_INCREF(obj), obj) #define __Pyx_Owned_Py_None(b) __Pyx_NewRef(Py_None) @@ -1365,7 +1353,7 @@ static CYTHON_INLINE Py_hash_t __Pyx_PyIndex_AsHash_t(PyObject*); #endif typedef Py_ssize_t __Pyx_compact_pylong; typedef size_t __Pyx_compact_upylong; - #else // Py < 3.12 + #else #define __Pyx_PyLong_IsNeg(x) (Py_SIZE(x) < 0) #define __Pyx_PyLong_IsNonNeg(x) (Py_SIZE(x) >= 0) #define __Pyx_PyLong_IsZero(x) (Py_SIZE(x) == 0) @@ -1769,8 +1757,8 @@ static CYTHON_INLINE int __Pyx_PyUnicode_Equals(PyObject* s1, PyObject* s2, int #define __Pyx_Arg_NewRef_VARARGS(arg) __Pyx_NewRef(arg) #define __Pyx_Arg_XDECREF_VARARGS(arg) Py_XDECREF(arg) #else - #define __Pyx_Arg_NewRef_VARARGS(arg) arg // no-op - #define __Pyx_Arg_XDECREF_VARARGS(arg) // no-op - arg is borrowed + #define __Pyx_Arg_NewRef_VARARGS(arg) arg + #define __Pyx_Arg_XDECREF_VARARGS(arg) #endif #define __Pyx_NumKwargs_VARARGS(kwds) PyDict_Size(kwds) #define __Pyx_KwValues_VARARGS(args, nargs) NULL @@ -1782,12 +1770,13 @@ static CYTHON_INLINE int __Pyx_PyUnicode_Equals(PyObject* s1, PyObject* s2, int #define __Pyx_KwValues_FASTCALL(args, nargs) ((args) + (nargs)) static CYTHON_INLINE PyObject * __Pyx_GetKwValue_FASTCALL(PyObject *kwnames, PyObject *const *kwvalues, PyObject *s); #if CYTHON_COMPILING_IN_CPYTHON && PY_VERSION_HEX >= 0x030d0000 - static CYTHON_UNUSED PyObject *__Pyx_KwargsAsDict_FASTCALL(PyObject *kwnames, PyObject *const *kwvalues); + CYTHON_UNUSED static PyObject *__Pyx_KwargsAsDict_FASTCALL(PyObject *kwnames, PyObject *const *kwvalues); #else #define __Pyx_KwargsAsDict_FASTCALL(kw, kwvalues) _PyStack_AsDict(kwvalues, kw) #endif - #define __Pyx_Arg_NewRef_FASTCALL(arg) arg // no-op, __Pyx_Arg_FASTCALL is direct and this needs - #define __Pyx_Arg_XDECREF_FASTCALL(arg) // no-op - arg was returned from array + #define __Pyx_Arg_NewRef_FASTCALL(arg) arg /* no-op, __Pyx_Arg_FASTCALL is direct and this needs + to have the same reference counting */ + #define __Pyx_Arg_XDECREF_FASTCALL(arg) #else #define __Pyx_Arg_FASTCALL __Pyx_Arg_VARARGS #define __Pyx_NumKwargs_FASTCALL __Pyx_NumKwargs_VARARGS @@ -1941,7 +1930,7 @@ typedef struct { #endif void *defaults; int defaults_pyobjects; - size_t defaults_size; // used by FusedFunction for copying defaults + size_t defaults_size; int flags; PyObject *defaults_tuple; PyObject *defaults_kwdict; @@ -4496,7 +4485,7 @@ static CYTHON_SMALL_CODE int __pyx_pymod_exec_collision_handler(PyObject *__pyx_ __pyx_t_1 = PyModule_Create(&__pyx_moduledef); if (unlikely(!__pyx_t_1)) __PYX_ERR(0, 1, __pyx_L1_error) { int add_module_result = PyState_AddModule(__pyx_t_1, &__pyx_moduledef); - __pyx_t_1 = 0; /* transfer ownership from __pyx_t_1 to collision_handler pseudovariable */ + __pyx_t_1 = 0; /* transfer ownership from __pyx_t_1 to "collision_handler" pseudovariable */ if (unlikely((add_module_result < 0))) __PYX_ERR(0, 1, __pyx_L1_error) pystate_addmodule_run = 1; } @@ -4946,7 +4935,7 @@ static CYTHON_INLINE PyObject *__Pyx_GetItemInt_Fast(PyObject *o, Py_ssize_t i, } } #else - if (is_list || PySequence_Check(o)) { + if (is_list || !PyMapping_Check(o)) { return PySequence_GetItem(o, i); } #endif @@ -5254,9 +5243,10 @@ static CYTHON_INLINE void __Pyx_RaiseNeedMoreValuesError(Py_ssize_t index) { /* IterFinish */ static CYTHON_INLINE int __Pyx_IterFinish(void) { + PyObject* exc_type; __Pyx_PyThreadState_declare __Pyx_PyThreadState_assign - PyObject* exc_type = __Pyx_PyErr_CurrentExceptionType(); + exc_type = __Pyx_PyErr_CurrentExceptionType(); if (unlikely(exc_type)) { if (unlikely(!__Pyx_PyErr_GivenExceptionMatches(exc_type, PyExc_StopIteration))) return -1; @@ -5475,14 +5465,14 @@ static CYTHON_INLINE PyObject * __Pyx_GetKwValue_FASTCALL(PyObject *kwnames, PyO { int eq = __Pyx_PyUnicode_Equals(s, PyTuple_GET_ITEM(kwnames, i), Py_EQ); if (unlikely(eq != 0)) { - if (unlikely(eq < 0)) return NULL; // error + if (unlikely(eq < 0)) return NULL; return kwvalues[i]; } } - return NULL; // not found (no exception set) + return NULL; } #if CYTHON_COMPILING_IN_CPYTHON && PY_VERSION_HEX >= 0x030d0000 -static CYTHON_UNUSED PyObject *__Pyx_KwargsAsDict_FASTCALL(PyObject *kwnames, PyObject *const *kwvalues) { +CYTHON_UNUSED static PyObject *__Pyx_KwargsAsDict_FASTCALL(PyObject *kwnames, PyObject *const *kwvalues) { Py_ssize_t i, nkwargs = PyTuple_GET_SIZE(kwnames); PyObject *dict; dict = PyDict_New(); @@ -5592,7 +5582,7 @@ static int __Pyx_ParseOptionalKeywords( if (*name) { values[name-argnames] = value; #if CYTHON_AVOID_BORROWED_REFS - Py_INCREF(value); // transfer ownership of value to values + Py_INCREF(value); Py_DECREF(key); #endif key = NULL; @@ -5611,7 +5601,7 @@ static int __Pyx_ParseOptionalKeywords( && _PyString_Eq(**name, key)) { values[name-argnames] = value; #if CYTHON_AVOID_BORROWED_REFS - value = NULL; // ownership transferred to values + value = NULL; #endif break; } @@ -5643,7 +5633,7 @@ static int __Pyx_ParseOptionalKeywords( if (cmp == 0) { values[name-argnames] = value; #if CYTHON_AVOID_BORROWED_REFS - value = NULL; // ownership transferred to values + value = NULL; #endif break; } @@ -6783,7 +6773,7 @@ static PyObject * __Pyx_CyFunction_Vectorcall_FASTCALL_KEYWORDS(PyObject *func, default: return NULL; } - return ((_PyCFunctionFastWithKeywords)(void(*)(void))def->ml_meth)(self, args, nargs, kwnames); + return ((__Pyx_PyCFunctionFastWithKeywords)(void(*)(void))def->ml_meth)(self, args, nargs, kwnames); } static PyObject * __Pyx_CyFunction_Vectorcall_FASTCALL_KEYWORDS_METHOD(PyObject *func, PyObject *const *args, size_t nargsf, PyObject *kwnames) { @@ -7268,7 +7258,7 @@ static PyCodeObject* __Pyx_CreateCodeObjectForTraceback( #else py_code = PyCode_NewEmpty(filename, funcname, py_line); #endif - Py_XDECREF(py_funcname); // XDECREF since it's only set on Py3 if cline + Py_XDECREF(py_funcname); return py_code; bad: Py_XDECREF(py_funcname); diff --git a/armer/cython/collision_handler.html b/armer/cython/collision_handler.html index d5beb2f..934c90b 100644 --- a/armer/cython/collision_handler.html +++ b/armer/cython/collision_handler.html @@ -1,5 +1,5 @@ - + @@ -364,7 +364,7 @@ -

Generated by Cython 3.0.5

+

Generated by Cython 3.0.9

Yellow lines hint at Python interaction.
Click on a line that starts with a "+" to see the C code that Cython generated for it. diff --git a/armer/robots/ROSRobot.py b/armer/robots/ROSRobot.py index d45053d..3c301e1 100644 --- a/armer/robots/ROSRobot.py +++ b/armer/robots/ROSRobot.py @@ -786,6 +786,12 @@ def pose_cb(self, goal: MoveToPoseGoal) -> None: goal_pose, ) pose = goal_pose.pose + + # Handle zero time input (or negative) + move_time = goal.time + if goal.time <= 0.0: + # Default to 5 seconds + move_time = 5.0 # Attempt to get valid solution # NOTE: on failure, returns existing state as solution for 0 movement @@ -797,7 +803,7 @@ def pose_cb(self, goal: MoveToPoseGoal) -> None: else: # NOTE: checks and preempts if collision or workspace violation # NOTE: can proceed if workspace not defined - if self.general_executor(q=solution.q, pose=pose, collision_ignore=False, workspace_ignore=False): + if self.general_executor(q=solution.q, pose=pose, collision_ignore=False, workspace_ignore=False, move_time_sec=move_time): self.pose_server.set_succeeded(MoveToPoseResult(success=True)) else: self.pose_server.set_aborted(MoveToPoseResult(success=False), 'Executor Failed in Action') @@ -817,10 +823,16 @@ def joint_pose_cb(self, goal: MoveToJointPoseGoal) -> None: self.preempt() with self.lock: + # Handle zero time input (or negative) + move_time = goal.time + if goal.time <= 0.0: + # Default to 5 seconds + move_time = 5.0 + # NOTE: checks for collisions and workspace violations # NOTE: checks for singularity violations # NOTE: can continue if workspace is not defined (ignored as no pose is defined) - if self.general_executor(q=goal.joints, workspace_ignore=True): + if self.general_executor(q=goal.joints, workspace_ignore=True, move_time_sec=move_time): self.joint_pose_server.set_succeeded(MoveToJointPoseResult(success=True)) else: self.joint_pose_server.set_aborted(MoveToJointPoseResult(success=False)) @@ -857,9 +869,15 @@ def named_pose_cb(self, goal: MoveToNamedPoseGoal) -> None: goal_pose.position.y = goal_pose_se3.t[1] goal_pose.position.z = goal_pose_se3.t[2] + # Handle zero time input (or negative) + move_time = goal.time + if goal.time <= 0.0: + # Default to 5 seconds + move_time = 5.0 + # NOTE: Checks collisions prior to executing # NOTE: Checks workspace (if defined), continues if not - if self.general_executor(q=q, pose=goal_pose): + if self.general_executor(q=q, pose=goal_pose, move_time_sec=move_time): self.named_pose_server.set_succeeded(MoveToNamedPoseResult(success=True)) else: self.named_pose_server.set_aborted(MoveToNamedPoseResult(success=False)) @@ -883,9 +901,15 @@ def home_cb(self, goal: HomeGoal) -> HomeResult: # Prep end goal state (q) for home joint positions q = np.array(self.qr) if hasattr(self, 'qr') else self.q + # Handle zero time input (or negative) + move_time = goal.time + if goal.time <= 0.0: + # Default to 5 seconds + move_time = 5.0 + # Run the general executor (checks for collisions) # NOTE: ignores workspace on homing - if self.general_executor(q=q, workspace_ignore=True): + if self.general_executor(q=q, workspace_ignore=True, move_time_sec=move_time): self.home_server.set_succeeded(HomeResult(success=True)) else: self.home_server.set_aborted(HomeResult(success=False)) @@ -2383,14 +2407,14 @@ def trajectory_collision_checker(self, traj) -> bool: # Output go based on passing collision check return go - def get_link_collision_dict(self) -> dict(): + def get_link_collision_dict(self) -> dict: """ Returns a dictionary of all associated links (names) which lists their respective collision data To be used by high-level armer class for collision handling """ return self.collision_dict - def check_collision_per_state(self, q: list() = []) -> bool(): + def check_collision_per_state(self, q: list = []) -> bool: """ Given a robot state (q) this method checks if the links (of a ghost robot) will result in a collision If a collision is found, then the output is True, else False @@ -2915,7 +2939,7 @@ def write_collision_scene_config(self, config_path: str = '') -> bool: # --------------------------------------------------------------------- # # --------- Standard Methods ------------------------------------------ # # --------------------------------------------------------------------- # - def general_executor(self, q, pose: Pose = None, collision_ignore: bool =False, workspace_ignore: bool = False) -> bool: + def general_executor(self, q, pose: Pose = None, collision_ignore: bool =False, workspace_ignore: bool = False, move_time_sec: float = 5.0) -> bool: """A general executor that performs the following on a given joint state goal - Workspace check prior to move. Setting workspace_ignore to True skips this (for cases where a pose is not defined or needed) - Singularity checking and termination on failure @@ -2930,6 +2954,8 @@ def general_executor(self, q, pose: Pose = None, collision_ignore: bool =False, :type collision_ignore: bool, optional :param workspace_ignore: True to ignore workspace checking, defaults to False :type workspace_ignore: bool, optional + :param move_time_sec: Time to take to move to pose, defaults to 5 seconds + :type move_time_sec: float, optional :return: True on success or False :rtype: bool """ @@ -2943,7 +2969,8 @@ def general_executor(self, q, pose: Pose = None, collision_ignore: bool =False, return result # Generate trajectory from successful solution - traj = self.traj_generator(self, qf=q) + rospy.loginfo(f"time to move: {move_time_sec}") + traj = self.traj_generator(self, qf=q, move_time_sec=move_time_sec) if traj.name == 'invalid': rospy.logwarn(f"[GENERAL EXECUTOR] -> Invalid trajectory detected, existing safely")