Kinematic Trajectory Optimization Skeleton PR #8

kamiradi · 2024-06-20T16:02:00Z

This PR aims to transcribe a basic Kinematic Trajectory Optimization problem from Moveit to Drake

Overload the PlannerManager class
Overload the PlanningContext class
Implement a ktopt_interface class

Implement an example demo to test and integrate the above classes
PS: this is a draft, and the work may be split over a sequence of PRs

This commit * solves the vcs import todo on dockerfile, Dockerfile is self-contained * compilation of motion plan request to trajectory optimization in progress

sea-bass

Looks good! I made some of the nitpicky comments.

As far as when I think we're at a mergeable state here: Ensure we can not only set up the KinematicTrajectoryOptimization program, but that we can actually solve it. Doesn't need to have any constraints. And then return the outcome as a planning response.

While I waited an eternity for the Docker container to keep building without OOMing my machine, I filled in some more placeholder code. Hope that's okay.

.docker/docker-compose.yaml

include/ktopt_interface/ktopt_planning_context.h

sea-bass · 2024-06-26T01:49:51Z

src/ktopt_planning_context.cpp

+    // compile into a Kinematic Trajectory Optimization problem
+    auto trajopt = KinematicTrajectoryOptimization(plant_->num_positions(), 10);


TODO: Rather than the hard-coded 10, the number of control points should be one of the generate_parameter_library params defined in the YAML... or otherwise in the planner options.

sea-bass · 2024-06-26T01:52:49Z

src/ktopt_planning_context.cpp

+    // bare minimum specification
+    // get the frame that needs constraints
+    //


Yep, so I think what we'll need is:

Must have:

Constraints on initial and final joint configuration

Joint position/velocity/acceleration limits from the joint limits params

Collision avoidance (minimum distance constraint)

Nice to have:

Getting position and orientation constraints from the MotionPlanningRequest

For both the collision and the position/orientation constraints, we'll also need a hyperparameter for how many samples from s=0 to s=1 we want to use. I would roughly recommend 2-3x the number of control points.

This can all be done in follow-on PRs, BTW

sea-bass · 2024-06-26T01:53:57Z

src/ktopt_planning_context.cpp

+        &builder, 0.0);
+
+    auto robot_instance = Parser(plant_, scene_graph_).AddModels(robot_description_);
+    plant_->WeldFrames(plant_->world_frame(), plant_->GetFrameByName("panda_link0"));


we'll need to figure out what exactly to weld here, since "panda_link0" is, of course, hard-coded. I'm sure there's a way to just weld the base-most link name of the robot description. That, or we can make it a parameter if needed, but hopefully isn't necessary.

creating issue.

sea-bass · 2024-06-26T04:35:19Z

Made more updates to the Dockerfile. Now I can do:

docker compose build
docker compose run moveit
$ colcon build --packages-select moveit_drake
Run the demo

And there are still no moveit apt packages installed. Ever! No need to remove any extra ones either :)

kamiradi

@sea-bass, I see that you have added the relevant bits to report the motion plan response. Were you able to test this on the demo?

sea-bass · 2024-07-03T11:01:38Z

@sea-bass, I see that you have added the relevant bits to report the motion plan response. Were you able to test this on the demo?

I was, but it's far from complete. There are no actual costs/constraints added, and the Drake trajectory needs to be converted to a MoveIt RobotTrajectory ans packed up into the result.

sjahr

Lots of small comments but in general this looks good! Once they are either addressed in the code or by converting the bigger ones into issues, we should merge this

sjahr · 2024-07-03T10:33:14Z

include/ktopt_interface/ktopt_interface.hpp

@@ -0,0 +1 @@
+#pragma once


Remove until we need it?

sjahr · 2024-07-03T10:33:33Z

include/ktopt_interface/ktopt_planning_context.hpp

+using drake::planning::trajectory_optimization::KinematicTrajectoryOptimization;
+using drake::solvers::Solve;
+
+class KTOptPlanningContext : public planning_interface::PlanningContext


TODO Add documentation

You mean add comments around the functions and includes?

creating issue.

include/ktopt_interface/ktopt_planning_context.hpp

ktopt_interface_plugin_description.xml

package.xml

sjahr · 2024-07-03T11:06:40Z

src/ktopt_planning_context.cpp

+  // retrieve goal state
+  moveit::core::RobotState goal_state(start_state);
+  constraint_samplers::ConstraintSamplerManager sampler_manager;
+  auto goal_sampler = sampler_manager.selectSampler(getPlanningScene(), getGroupName(), req.goal_constraints[0]);


I guess this will work for now, but maybe this step needs to be dropped and we let ktopt handle the goal constraints directly

sjahr · 2024-07-03T11:08:45Z

src/ktopt_planning_context.cpp

+
+  // package up the resulting trajectory
+  auto traj = trajopt.ReconstructTrajectory(result);
+  const size_t num_pts = 101;  // TODO: should be sample time based instead


I guess this could be exposed as a ROS parameter to let the use define the trajectory density

Completely agreed! That was going to be the next step.

sjahr · 2024-07-03T11:09:11Z

src/ktopt_planning_context.cpp

+
+bool KTOptPlanningContext::terminate()
+{
+  return true;


Print a warning here that termination is not implemented by this planner

sjahr · 2024-07-03T11:10:15Z

src/ktopt_planning_context.cpp

+  robot_description_ = robot_description;
+
+  // also perform some drake related initialisations here
+  DiagramBuilder<double> builder;
+
+  std::tie(plant_, scene_graph_) = AddMultibodyPlantSceneGraph(&builder, 0.0);
+
+  auto robot_instance = Parser(plant_, scene_graph_).AddModels(robot_description_);
+  plant_->WeldFrames(plant_->world_frame(), plant_->GetFrameByName("panda_link0"));
+
+  // for now finalize plant here
+  plant_->Finalize();


I wonder if this (in a follow-up PR) should be moved into the constructor or even the planner manager

Again, I declared this as a function because , there could be use in resetting the robot_description once a planning_context has been set. The alternative is to expose a ros_handler and pass it to the planner_manager. I'll experiment with that in a future pass.

sjahr · 2024-07-03T11:10:34Z

src/ktopt_planning_context.cpp

+
+void KTOptPlanningContext::clear()
+{
+  // do something


Just print warning that is it not implemented for now 😉

sea-bass · 2024-07-03T11:46:24Z

README.md

+### Some helper commands
+To just build `moveit_drake`
+```
+colcon build --packages-select mvoeit_drake


Typo: moveit_drake

kamiradi

So I have made a few changes to the repository based on your comments and create a few follow on issues to be tracked with later commits.

CMakeLists.txt

sjahr · 2024-07-03T15:19:03Z

@kamiradi Can you ping when you addressed all comments for another review round? You can resolve the conversation you addressed and comment on the ones where you might be unsure/ have questions or commetns

kamiradi · 2024-07-04T08:02:04Z

@sjahr, I remember you pointing me towards a specific yaml file that needs to have a _planning suffix to be loaded by the class loader. Could you point me towards an example? I believe this is needed for the drake planning plugin to be loaded during runtime.

sjahr · 2024-07-04T08:11:36Z

@sjahr, I remember you pointing me towards a specific yaml file that needs to have a _planning suffix to be loaded by the class loader. Could you point me towards an example? I believe this is needed for the drake planning plugin to be loaded during runtime.

@kamiradi The file is specified per moveit config, for example in panda_moveit_config for stomp:
https://github.com/moveit/moveit_resources/blob/ros2/panda_moveit_config/config/stomp_planning.yaml

The content is equivalent with the section you've added to the launch file and either the yaml file or the launch file section can be used (although the .yaml file is best practices for fleshed out plugins).

kamiradi · 2024-07-04T08:23:01Z

Ah I see, so this is an alternative to the launch file config mentioned in the launch file. Is there a runtime requirement for the yaml file also to be present? Can I defer this to the next PR?

So the requirement now is to:

Declare a similar ktopt_planning.yaml file (where should I be doing this? should I create a new config folder within the repository?)
And the class loader automagically loads this at runtime.

kamiradi · 2024-07-04T08:28:02Z

demo/launch/pipeline_testbench.launch.py

@sea-bass @sjahr help needed with the drake moveit pipeline demo test. Creating a separate issue to track this

I found a typo in the XML, that "planning_interface:PlannerManager" needs to have :: instead of a single :. There was also a missing space, though unclear if that does anything.

Fixed in 2ff0c3b -- try with that?

sea-bass

Approving pending @sjahr's final review.

My only suggestion: Rename the PR title to something like "Kinematic Trajectory Optimization skeleton"

sjahr · 2024-07-10T08:06:42Z

CMakeLists.txt

+                      ktopt_moveit_parameters)
+
+# Ensure that the plugin finds libdrake.so at runtime
+set_target_properties(


@kamiradi @sea-bass I needed to set these properties to avoid a segfault when the plugin was loaded because it couldn't find libdrake at runtime. Let me know what you think

sjahr

Works for me now 🎉

kamiradi added 6 commits June 20, 2024 16:56

[FT] adds docker image and compose file

9c6e420

[WIP] adds ktopt header and src files

3cc7290

[WIP] ktopt initial changes

bad9ae5

[WIP] adds plugin to CMakeLists

4d418d5

[WIP] reflects stomp template

b0bf263

[WIP] build success, setting up optimization

5a49354

This commit * solves the vcs import todo on dockerfile, Dockerfile is self-contained * compilation of motion plan request to trajectory optimization in progress

sea-bass requested review from sea-bass and sjahr June 25, 2024 12:27

sea-bass added 3 commits June 25, 2024 21:37

Remove extra compose file

cb33b01

nits and pre-commit

d2913f9

Dockerfile fixes and some more updates in the planning context

cdae97e

sea-bass reviewed Jun 26, 2024

View reviewed changes

sea-bass added 2 commits June 25, 2024 22:52

Remove bad include

7761068

Fix Docker build

4698eaf

sea-bass force-pushed the ktopt_new branch from 3f05814 to 4698eaf Compare June 26, 2024 04:09

kamiradi commented Jul 3, 2024

View reviewed changes

sjahr reviewed Jul 3, 2024

View reviewed changes

solves merge conflicts

c61ddc1

This was referenced Jul 3, 2024

Change trajectory related config options in pipeline.launch.py #9

Closed

Remove from planner config #10

Closed

Optimal SceneGraph pointer #11

Closed

Add a error message if robot_description is set prematurely. #12

Open

sea-bass reviewed Jul 3, 2024

View reviewed changes

[FIX] fixes readme typo

bb9cc81

kamiradi mentioned this pull request Jul 3, 2024

Replace declaration lambdas. #13

Closed

[FIX] changes planning description file to reflect ROS2 specification

24e0c3e

kamiradi commented Jul 3, 2024

View reviewed changes

CMakeLists.txt Outdated Show resolved Hide resolved

[FIX] install include directory

75b42c8

This was referenced Jul 4, 2024

[Cleanup] Add documentation around the code. #14

Closed

Add relevant parameters to the drake_moveit.yaml file #15

Closed

kamiradi added 3 commits July 4, 2024 08:38

[FIX] changes default to true

66a0f3c

[FIX] changes variable qualifier to const

138d6ac

[FIX] not implemented warning messages

e804235

kamiradi requested a review from sjahr July 4, 2024 08:02

kamiradi mentioned this pull request Jul 4, 2024

Remove hard coded frames, retrieve from parameters #16

Closed

[WIP] tries to execute the demo with changed pipeline config

4a31afe

kamiradi commented Jul 4, 2024

View reviewed changes

kamiradi mentioned this pull request Jul 4, 2024

Demo launch file debug #17

Closed

Fix typo in plugin name

2ff0c3b

sjahr marked this pull request as ready for review July 9, 2024 12:35

sea-bass approved these changes Jul 9, 2024

View reviewed changes

Some cleanups + CMake fixes

ea2632c

sjahr reviewed Jul 10, 2024

View reviewed changes

sjahr approved these changes Jul 10, 2024

View reviewed changes

sjahr merged commit 345e52c into moveit:main Jul 10, 2024
1 check passed

kamiradi changed the title ~~Kinematic Trajectory Optimization draft PR~~ Kinematic Trajectory Optimization Skeleton PR Jul 10, 2024

kamiradi deleted the ktopt_new branch July 11, 2024 08:40

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Kinematic Trajectory Optimization Skeleton PR #8

Kinematic Trajectory Optimization Skeleton PR #8

kamiradi commented Jun 20, 2024 •

edited

Loading

sea-bass left a comment

sea-bass Jun 26, 2024

sea-bass Jun 26, 2024

sea-bass Jun 26, 2024

kamiradi Jul 4, 2024

sea-bass commented Jun 26, 2024

kamiradi left a comment

sea-bass commented Jul 3, 2024

sjahr left a comment

sjahr Jul 3, 2024

sjahr Jul 3, 2024

kamiradi Jul 3, 2024

sjahr Jul 3, 2024

kamiradi Jul 4, 2024

sjahr Jul 3, 2024

sjahr Jul 3, 2024

kamiradi Jul 3, 2024

sjahr Jul 3, 2024

sjahr Jul 3, 2024

kamiradi Jul 3, 2024

sjahr Jul 3, 2024

sea-bass Jul 3, 2024

kamiradi left a comment

sjahr commented Jul 3, 2024

kamiradi commented Jul 4, 2024

sjahr commented Jul 4, 2024 •

edited

Loading

kamiradi commented Jul 4, 2024

kamiradi Jul 4, 2024 •

edited

Loading

sea-bass Jul 5, 2024 •

edited

Loading

sea-bass left a comment

sjahr Jul 10, 2024

sjahr left a comment

		// compile into a Kinematic Trajectory Optimization problem
		auto trajopt = KinematicTrajectoryOptimization(plant_->num_positions(), 10);

Kinematic Trajectory Optimization Skeleton PR #8

Kinematic Trajectory Optimization Skeleton PR #8

Conversation

kamiradi commented Jun 20, 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

sea-bass commented Jun 26, 2024

kamiradi left a comment

Choose a reason for hiding this comment

sea-bass commented Jul 3, 2024

sjahr 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

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

kamiradi left a comment

Choose a reason for hiding this comment

sjahr commented Jul 3, 2024

kamiradi commented Jul 4, 2024

sjahr commented Jul 4, 2024 • edited Loading

kamiradi commented Jul 4, 2024

kamiradi Jul 4, 2024 • edited Loading

Choose a reason for hiding this comment

sea-bass Jul 5, 2024 • edited Loading

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

sjahr left a comment

Choose a reason for hiding this comment

kamiradi commented Jun 20, 2024 •

edited

Loading

sjahr commented Jul 4, 2024 •

edited

Loading

kamiradi Jul 4, 2024 •

edited

Loading

sea-bass Jul 5, 2024 •

edited

Loading