[jog_arm] a ROS service to enable task redundancy #1855
Conversation
AndyZe
changed the title
moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h
Outdated
Show resolved
Hide resolved
moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h
Outdated
Show resolved
Hide resolved
moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h
Outdated
Show resolved
Hide resolved
|
CI tests are failing because this PR to moveit_msgs hasn't been merged yet. moveit/moveit_msgs#63 |
Codecov Report
@@ Coverage Diff @@
## master #1855 +/- ##
==========================================
+ Coverage 50.26% 50.29% +0.02%
==========================================
Files 313 313
Lines 24623 24652 +29
==========================================
+ Hits 12377 12398 +21
- Misses 12246 12254 +8
Continue to review full report at Codecov.
|
moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h
Show resolved
Hide resolved
...erimental/moveit_jog_arm/test/test_drift_dimensions_service/test_drift_dimensions_service.py
Outdated
Show resolved
Hide resolved
It only fails on CI, fine locally
| @@ -79,6 +79,10 @@ struct JogArmShared | |||
|
|
|||
| // The transform from the MoveIt planning frame to robot_link_command_frame | |||
| Eigen::Isometry3d tf_moveit_to_cmd_frame; | |||
|
|
|||
| // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] | |||
| std::atomic_bool drift_dimensions[6] = { ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), | |||
There was a problem hiding this comment.
Any reason to use c-style arrays rather than a std::array<std::atomic_bool, 6>
There was a problem hiding this comment.
Yeah, it's not so easy to initialize a vector of std::atomic's to default values. It took me a lot of stackexchange browsing to figure this out. Something like this doesn't work:
std::vector<std::atomic_bool> test = {ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false)};
The compiler error is:
error: use of deleted function ‘std::atomic<bool>::atomic(const std::atomic<bool>&)’ { ::new(static_cast<void*>(__p)) _T1(std::forward<_Args>(__args)...); }
Also, it's always going to have a length of 6, so you don't need a lot of the functionality of a std::vector.
| // Work backwards through the 6-vector so indices don't get out of order | ||
| for (auto dimension = jacobian_.rows(); dimension >= 0; --dimension) | ||
| { | ||
| if (shared_variables.drift_dimensions[dimension] && jacobian_.rows() > 1) |
There was a problem hiding this comment.
Can you check jacobian_.rows() > 1 once outside the loop instead?
There was a problem hiding this comment.
Good idea but I don't think so, because you might be removing a row in every iteration
There was a problem hiding this comment.
Well, I guess I could move it out if I counted the number of true's in shared_variables.drift_dimensions. Meh, that might be more trouble than it's worth
moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h
Outdated
Show resolved
Hide resolved
| * @param delta_x Vector of Cartesian delta commands, should be 6-long. | ||
| * @param row_to_remove Dimension that will be allowed to drift, e.g. row_to_remove = 2 allows z-translation drift. | ||
| */ | ||
| void removeDimension(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x, unsigned int row_to_remove); |
There was a problem hiding this comment.
Seems like this should be two different methods:
| void removeDimension(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x, unsigned int row_to_remove); | |
| void removeDimension(Eigen::MatrixXd& matrix, unsigned int row_to_remove); | |
| void removeDimension(Eigen::VectorXd& delta_x, unsigned int index_to_remove); |
There was a problem hiding this comment.
I'm not a fan of 2 separate methods because:
-
you need to call both methods or there will be a size mismatch when you try to multiply them
-
it's a little bit more efficient this way (same for-loop)
Add a ROS service to turn redundancy on/off in specific dimensions. For example, maybe you could allow wrist rotation by
drift_x_rotation == true. For now, the reference frame is the jogging command frame.This depends on this moveit_msgs PR
Here are 2 gif's starting from a singular position, without and with allowing drift respectively. It's trying to move in the world Y-direction. The first gif shows it stuck. The second gif moves past the singularity, no problem.
