Add utility functions to get limits and trajectory message #2861
Conversation
moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
Outdated
Show resolved
Hide resolved
...jectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h
Outdated
Show resolved
Hide resolved
moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
Outdated
Show resolved
Hide resolved
moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
Outdated
Show resolved
Hide resolved
| Eigen::VectorXd lower_limits(group.getActiveVariableCount()); | ||
| Eigen::VectorXd upper_limits(group.getActiveVariableCount()); | ||
| const moveit::core::JointBoundsVector& group_bounds = group.getActiveJointModelsBounds(); | ||
| int joint_index = 0; |
There was a problem hiding this comment.
Would variable_index be more correct?
There was a problem hiding this comment.
Not sure but I like joint index better, are you fine with keeping it like this?
There was a problem hiding this comment.
The reason why I'm proposing variable_index is because a joint can have multiple variables and this function iterates over those variables, not over joints.
| for (std::size_t i = 0; i < bounds.size(); ++i) | ||
| { | ||
| assert(bounds[i]->size() == 1); // single-variable joints. | ||
| max_joint_velocities[i] = std::min(-bounds[i]->at(0).min_velocity_, bounds[i]->at(0).max_velocity_); |
There was a problem hiding this comment.
If max. velocities or accelerations are not symmetric (e.g. [-1, 2]), this function will return the most limiting element (i.e. 1). Maybe worth clarifying that in the doc in the header, and maybe rename the function to getMostLimitingSymmetricVelocitiesAndAccelerations, which I realize it's probably too long...
There was a problem hiding this comment.
I named it getMaxVelocitiesAndAccelerationBounds what do you think about that?
moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
Outdated
Show resolved
Hide resolved
| Eigen::VectorXd lower_limits(group.getActiveVariableCount()); | ||
| Eigen::VectorXd upper_limits(group.getActiveVariableCount()); | ||
| const moveit::core::JointBoundsVector& group_bounds = group.getActiveJointModelsBounds(); | ||
| int joint_index = 0; |
There was a problem hiding this comment.
The reason why I'm proposing variable_index is because a joint can have multiple variables and this function iterates over those variables, not over joints.
moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
Outdated
Show resolved
Hide resolved
moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
Outdated
Show resolved
Hide resolved
moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h
Outdated
Show resolved
Hide resolved
Co-authored-by: Mario Prats <mario.prats@picknik.ai>
Co-authored-by: Mario Prats <mario.prats@picknik.ai>
Description
Please explain the changes you made, including a reference to the related issue if applicable
Checklist