README clean up and integration of Kinematic limits from Robot Model #49

kamiradi · 2024-09-18T14:16:50Z

This PR addresses #45.

Adds Position, Velocity and Acceleration limits from moveit - variable bounds
Adds Jerk bounds from parameter header
Cleans up README.md

This commit adds upper/lower position/velocity bounds. Reads them from the robot model VariableBound API and sets them up as Drake PositionBund and VelocityBound constraints. Adds some additional docs on readme

sea-bass

Might be easier to break this up task by task, so it's faster to review smaller things and get them in. Would recommend finishing up what you've got so far and then following this suggestion.

sea-bass · 2024-09-20T05:53:29Z

README.md

+- Exposes `KinematicTrajectoryOptimization` implementation in `drake`.
+- Exposes `TOPPRA` implementation in `drake`.


consider links to Drake docs

sea-bass · 2024-09-20T05:53:39Z

res/ktopt_moveit_parameters.yaml

+    # validation: {
+    #   gt_eq<>: [1]


sea-bass · 2024-09-20T05:53:49Z

src/ktopt_planning_context.cpp

+    // std::cout << "Joint " << joint_name << ": Position ["
+    //           << bounds.min_position_ << ", " << bounds.max_position_
+    //           << "], Velocity [-" << bounds.max_velocity_ << ", "
+    //           << bounds.max_velocity_ << "]" << std::endl;


sea-bass · 2024-09-20T05:54:21Z

src/ktopt_planning_context.cpp

+  int num_positions = plant.num_positions();
+  int num_velocities = plant.num_velocities();


can be const

sea-bass · 2024-09-20T05:54:46Z

src/ktopt_planning_context.cpp

+  int num_velocities = plant.num_velocities();
+
+  // Ensure the bounds have the correct size
+  if (lower_position_bounds.size() != num_positions || upper_position_bounds.size() != num_positions)


you might get compiler warnings in comparing int with size_t, which requires a static_cast to resolve

sea-bass

Left some additional comments, but once addressed this is good for me, so approving.

sea-bass · 2024-10-04T10:15:42Z

README.md

+
+`moveit_drake` brings together the vertical ROS integration of the
+[MoveIt2](https://moveit.ai/) motion planning framework, with the Mathematical
+Programming interface within [drake](https://drake.mit.edu/). This allows the


Capitalize Drake

sea-bass · 2024-10-04T10:15:56Z

README.md

+release.
+
+`moveit_drake` brings together the vertical ROS integration of the
+[MoveIt2](https://moveit.ai/) motion planning framework, with the Mathematical


MoveIt 2 (with a space)

sea-bass · 2024-10-04T10:16:47Z

README.md

-### Potential issues
- Assumes that planner managers initialize will set robot description before a
-  call to getPlanningContext.
+- Use [pre-commit to format your


Might want to share the basic command

pre-commit run -a

sea-bass · 2024-10-04T10:17:40Z

src/ktopt_planning_context.cpp

@@ -48,6 +48,62 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
  const auto joint_model_group = getPlanningScene()->getRobotModel()->getJointModelGroup(getGroupName());
  RCLCPP_INFO_STREAM(getLogger(), "Planning for group: " << getGroupName());

+  // extract position and velocity bounds
+  std::vector<double> lower_position_bounds;


Try initialize all of these with the known size (number of dofs) for efficiency.

If you initialize them, change the push_backs for modifying existing elements.

Or you can reserve() them in which case you do keep pushing back.

sea-bass · 2024-10-04T10:18:10Z

src/ktopt_planning_context.cpp

+  std::vector<double> upper_acceleration_bounds;
+  for (const auto& joint_model : joint_model_group->getActiveJointModels())
+  {
+    const std::string& joint_name = joint_model->getName();


This and the next line can just use auto& for the return type

sea-bass · 2024-10-04T10:19:33Z

src/ktopt_planning_context.cpp

+    lower_acceleration_bounds.push_back(-bounds.max_acceleration_);
+    upper_acceleration_bounds.push_back(bounds.max_acceleration_);
+  }
+  const int num_positions = plant.num_positions();


This and the next line can just be auto

sea-bass · 2024-10-04T10:22:15Z

src/ktopt_planning_context.cpp

+  // Ensure the bounds have the correct size
+  if (lower_position_bounds.size() != num_positions || upper_position_bounds.size() != num_positions)
+  {
+    lower_position_bounds.resize(num_positions, -1e6);


Instead of using these hard-coded 1e6 magic numbers, consider defining them as constants in an anonymous namespace.

namespace { constexpr double kDefaultJointMaxPosition = 1.0e6; } // namespace

But more importantly, did you find that you needed to pad these vectors? Is this because of the additional joints that are not part of the group?

If that's the case, then you possibly want these extra joints to have big position limits (as you did), but also to use zero vel/accel/jerk limits to ensure they do not move.

Or possibly the position limits just have their min/max set to the current state? Might be even better

This was a good catch. On closer inspection, I completely missed that the inactive joints were having their kinematics limits added to the end of each respective (position, vel, accel, jerk) bound vector, instead of being indexed. Corrected now!

sea-bass · 2024-10-04T10:26:16Z

And also, can you change the PR name to more precisely say what this ends up adding? Cleaned up README and kinematic limits from the robot model?

kamiradi added 4 commits September 18, 2024 15:15

adds urdf as a param

c873ba5

adds a small introduction on the readme

f15c196

[FT] Sets up pos/vel bounds in KTOPT

ec9acd3

This commit adds upper/lower position/velocity bounds. Reads them from the robot model VariableBound API and sets them up as Drake PositionBund and VelocityBound constraints. Adds some additional docs on readme

pre-commits

9c59be2

sea-bass reviewed Sep 20, 2024

View reviewed changes

kamiradi added 2 commits September 20, 2024 15:49

addresses review comments

f2202db

pre-commit stuff

61e4381

sjahr linked an issue Sep 24, 2024 that may be closed by this pull request

Remove dependency on hardcoded URDF #43

Closed

adds acceleration and place holder jerk constraint

e5ab7f4

kamiradi requested a review from sea-bass October 4, 2024 08:17

sea-bass approved these changes Oct 4, 2024

View reviewed changes

kamiradi changed the title ~~Addresses roscon task list~~ README clean up and integration of Kinematic limits from Robot Model Oct 4, 2024

kamiradi added 2 commits October 4, 2024 15:44

initialises bounds with dofs by reserving memory -

75d73ed

[FIX] fixes incorrect integration of inactive kinematic limits

a88ce75

kamiradi marked this pull request as ready for review October 4, 2024 14:58

kamiradi merged commit bf1bf0b into moveit:main Oct 4, 2024
2 checks passed

kamiradi deleted the roscon_issues branch October 4, 2024 15:06

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

README clean up and integration of Kinematic limits from Robot Model #49

README clean up and integration of Kinematic limits from Robot Model #49

kamiradi commented Sep 18, 2024 •

edited

Loading

sea-bass left a comment

sea-bass Sep 20, 2024

sea-bass Sep 20, 2024

sea-bass Sep 20, 2024

sea-bass Sep 20, 2024

sea-bass Sep 20, 2024

sea-bass left a comment

sea-bass Oct 4, 2024

sea-bass Oct 4, 2024

sea-bass Oct 4, 2024

sea-bass Oct 4, 2024

sea-bass Oct 4, 2024

sea-bass Oct 4, 2024

sea-bass Oct 4, 2024 •

edited

Loading

kamiradi Oct 4, 2024

sea-bass commented Oct 4, 2024

		- Exposes `KinematicTrajectoryOptimization` implementation in `drake`.
		- Exposes `TOPPRA` implementation in `drake`.

		int num_positions = plant.num_positions();
		int num_velocities = plant.num_velocities();

README clean up and integration of Kinematic limits from Robot Model #49

README clean up and integration of Kinematic limits from Robot Model #49

Conversation

kamiradi commented Sep 18, 2024 • edited Loading

sea-bass left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass Oct 4, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

sea-bass commented Oct 4, 2024

kamiradi commented Sep 18, 2024 •

edited

Loading

sea-bass Oct 4, 2024 •

edited

Loading