Conversation
There was a problem hiding this comment.
Pull request overview
This pull request adds support for the UR5 robot to the motion planning framework for MBM (Motion Benchmark) evaluation. The UR5 is a 6-DOF industrial robotic arm. The implementation follows the existing pattern used for other robots (Panda, Fetch, Baxter) by providing CUDA device code for forward kinematics and collision checking, along with robot configuration parameters.
Changes:
- Added complete UR5 robot implementation with collision sphere approximations and forward kinematics
- Integrated UR5 into the planning pipeline and evaluation scripts
- Added robot struct definition with joint limits and scaling functions
Reviewed changes
Copilot reviewed 5 out of 5 changed files in this pull request and generated 4 comments.
Show a summary per file
| File | Description |
|---|---|
| src/robots/ur5.cuh | New file implementing UR5 forward kinematics, collision checking (both approximate with 17 spheres and detailed with 40 spheres), and robot-specific constants |
| src/planning/pRRTC.cu | Added include for ur5.cuh and template instantiation for UR5 with 6 DOF |
| src/planning/Robots.hh | Added Ur5 struct with dimension=6, joint scaling parameters, and configuration utilities |
| scripts/single_mbm.cpp | Added ur5 case to robot type selection for single problem evaluation |
| scripts/evaluate_mbm.cpp | Added ur5 case to robot type selection for batch problem evaluation |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
| #define FIXED -1 | ||
| #define X_PRISM 0 | ||
| #define Y_PRISM 1 | ||
| #define Z_PRISM 2 | ||
| #define X_ROT 3 | ||
| #define Y_ROT 4 | ||
| #define Z_ROT 5 | ||
| #define BATCH_SIZE 16 |
There was a problem hiding this comment.
Duplicate macro definitions. The macros FIXED, X_PRISM, Y_PRISM, Z_PRISM, X_ROT, Y_ROT, Z_ROT, and BATCH_SIZE are defined again at lines 330-337. Since macros don't respect namespaces in C/C++, these redefinitions will cause compiler warnings about macro redefinition. These duplicate definitions should be removed since they're already defined in utils.cuh (which is included before this file) and don't need to be repeated.
| #define FIXED -1 | |
| #define X_PRISM 0 | |
| #define Y_PRISM 1 | |
| #define Z_PRISM 2 | |
| #define X_ROT 3 | |
| #define Y_ROT 4 | |
| #define Z_ROT 5 | |
| #define BATCH_SIZE 16 |
| }; | ||
|
|
||
| __device__ __constant__ int ur5_approx_joint_id_to_dof[7] = { | ||
| 18446744073709551615, |
There was a problem hiding this comment.
Invalid integer value for int array. The value 18446744073709551615 (2^64 - 1) exceeds the maximum value for a signed 32-bit int (2147483647). This will cause integer overflow and undefined behavior. The array is declared as 'int' but this value is only valid for unsigned 64-bit integers. Since this appears to be a sentinel value for a fixed joint (not controlled by a degree of freedom), it should be -1 instead, which is the conventional sentinel value used elsewhere in the codebase (as seen in FIXED macro and flattened_joint_to_spheres arrays).
| 18446744073709551615, | |
| FIXED, |
| #define FIXED -1 | ||
| #define X_PRISM 0 | ||
| #define Y_PRISM 1 | ||
| #define Z_PRISM 2 | ||
| #define X_ROT 3 | ||
| #define Y_ROT 4 | ||
| #define Z_ROT 5 | ||
| #define BATCH_SIZE 16 |
There was a problem hiding this comment.
Duplicate macro definitions. The macros FIXED, X_PRISM, Y_PRISM, Z_PRISM, X_ROT, Y_ROT, Z_ROT, and BATCH_SIZE are redefined here. Since these are already defined earlier in the same file (lines 10-17) and in utils.cuh, these redefinitions will cause compiler warnings about macro redefinition. These duplicate definitions should be removed.
| #define FIXED -1 | |
| #define X_PRISM 0 | |
| #define Y_PRISM 1 | |
| #define Z_PRISM 2 | |
| #define X_ROT 3 | |
| #define Y_ROT 4 | |
| #define Z_ROT 5 | |
| #define BATCH_SIZE 16 |
| }; | ||
|
|
||
| __device__ __constant__ int ur5_joint_id_to_dof[7] = { | ||
| 18446744073709551615, |
There was a problem hiding this comment.
Invalid integer value for int array. The value 18446744073709551615 (2^64 - 1) exceeds the maximum value for a signed 32-bit int (2147483647). This will cause integer overflow and undefined behavior. The array is declared as 'int' but this value is only valid for unsigned 64-bit integers. Since this appears to be a sentinel value for a fixed joint (not controlled by a degree of freedom), it should be -1 instead, which is the conventional sentinel value used elsewhere in the codebase.
| 18446744073709551615, | |
| FIXED, |
|
hello! could you also add in the MBM json files for ur5? |
|
It's been added in the ur5_support branch! |
Added ur5 support for mbm evaluation