Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Merged
merged 9 commits into from
Oct 4, 2024
Merged

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

Show file tree
Hide file tree
Changes from 4 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
c873ba5
adds urdf as a param
kamiradi Sep 18, 2024
f15c196
adds a small introduction on the readme
kamiradi Sep 18, 2024
ec9acd3
[FT] Sets up pos/vel bounds in KTOPT
kamiradi Sep 19, 2024
9c59be2
pre-commits
kamiradi Sep 19, 2024
f2202db
addresses review comments
kamiradi Sep 20, 2024
61e4381
pre-commit stuff
kamiradi Sep 20, 2024
e5ab7f4
adds acceleration and place holder jerk constraint
kamiradi Oct 4, 2024
75d73ed
initialises bounds with dofs by reserving memory -
kamiradi Oct 4, 2024
a88ce75
[FIX] fixes incorrect integration of inactive kinematic limits
kamiradi Oct 4, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 20 additions & 25 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,24 @@
# Experimental MoveIt 2 - Drake Integration

Under construction
NOTE: Experimental and will continue to have breaking changes until first
release.

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

MoveIt 2 (with a space)

Programming interface within [drake](https://drake.mit.edu/). This allows the
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Capitalize Drake

user to setup motion planning as an optimization problem within ROS, with the
rich specification of constraints and costs provided by `drake`.

## Features

- Exposes `KinematicTrajectoryOptimization` implementation in `drake`.
- Exposes `TOPPRA` implementation in `drake`.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

consider links to Drake docs


## Docker Workflow (Preferred and tested)

### Requirements
`docker` and `docker-compose` - Follow instructions [here](https://docs.docker.com/engine/install/ubuntu/).
`docker` and `docker-compose` - Follow instructions
[here](https://docs.docker.com/engine/install/ubuntu/).

### Steps
The following steps clone and build the base image that you will require to
Expand Down Expand Up @@ -33,7 +46,9 @@ Follow [instructions](#build-moveit_drake) below to build `moveit_drake`

### Build `moveit_drake`

Follow the [MoveIt Source Build](https://moveit.ros.org/install-moveit2/source/) instructions to set up a colcon workspace with MoveIt from source.
Follow the [MoveIt Source
Build](https://moveit.ros.org/install-moveit2/source/) instructions to set up a
colcon workspace with MoveIt from source.

Open a command line to your colcon workspace:

Expand All @@ -58,28 +73,8 @@ ros2 launch moveit_drake pipeline_testbench.launch.py

### Development

- Use [pre-commit to format your code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks)

# Todo section

This section keeps a list of immediate todos, will be deleted before repo release

- [x] Create drake planning pipeline option in `pipeline_testbench.launch.py`
- [x] Declare to moveit, to use the drake ktopt planning pipeline
- [ ] Build planner manager and planning context to display info from `moveit`
and `drake` instance.
- [ ] Generated placeholder classes mimicking `stomp` implementation.
- [ ] Display info messages during testbench runtime.
- [ ]
- [ ] read Robot description and display onto drake visualizer

### Doubts
- [x] stomp_moveit::ParamListener, where is this being declared
- [ ] Why is the parameter file in the "res" directory

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Might want to share the basic command

pre-commit run -a

code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks)

### Some helper commands
To just rebuild `moveit_drake`
Expand Down
8 changes: 8 additions & 0 deletions res/ktopt_moveit_parameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,12 @@
ktopt_interface:
drake_robot_description: {
type: string,
description: "Robot description to be loaded by the internal Drake MultibodyPlant",
default_value: "package://drake_models/franka_description/urdf/panda_arm_hand.urdf",
# validation: {
# gt_eq<>: [1]
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

remove

# }
}
num_iterations: {
type: int,
description: "Number of iterations for the Drake mathematical program solver.",
Expand Down
56 changes: 47 additions & 9 deletions src/ktopt_planning_context.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,50 @@ 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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

std::vector<double> upper_position_bounds;
std::vector<double> lower_velocity_bounds;
std::vector<double> upper_velocity_bounds;
for (const auto& joint_model : joint_model_group->getActiveJointModels())
{
const std::string& joint_name = joint_model->getName();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

const moveit::core::VariableBounds& bounds = joint_model->getVariableBounds()[0];

lower_position_bounds.push_back(bounds.min_position_);
upper_position_bounds.push_back(bounds.max_position_);
lower_velocity_bounds.push_back(-bounds.max_velocity_);
upper_velocity_bounds.push_back(bounds.max_velocity_);

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

remove

}
int num_positions = plant.num_positions();
int num_velocities = plant.num_velocities();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can be const


// Ensure the bounds have the correct size
if (lower_position_bounds.size() != num_positions || upper_position_bounds.size() != num_positions)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

{
lower_position_bounds.resize(num_positions, -1e6);
Copy link
Contributor

@sea-bass sea-bass Oct 4, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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!

upper_position_bounds.resize(num_positions, 1e6);
}
if (lower_velocity_bounds.size() != num_velocities || upper_velocity_bounds.size() != num_velocities)
{
// Resize velocity bounds to match number of velocities, if necessary
lower_velocity_bounds.resize(num_velocities, -1e6);
upper_velocity_bounds.resize(num_velocities, 1e6);
}
Eigen::Map<const Eigen::VectorXd> lower_position_bounds_eigen(lower_position_bounds.data(),
lower_position_bounds.size());
Eigen::Map<const Eigen::VectorXd> upper_position_bounds_eigen(upper_position_bounds.data(),
upper_position_bounds.size());
Eigen::Map<const Eigen::VectorXd> lower_velocity_bounds_eigen(lower_velocity_bounds.data(),
lower_velocity_bounds.size());
Eigen::Map<const Eigen::VectorXd> upper_velocity_bounds_eigen(upper_velocity_bounds.data(),
upper_velocity_bounds.size());

// q represents the complete state (joint positions and velocities)
VectorXd q = VectorXd::Zero(plant.num_positions() + plant.num_velocities());
q << moveit::drake::getJointPositionVector(start_state, getGroupName(), plant);
Expand Down Expand Up @@ -97,12 +141,8 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res)
trajopt.AddPathVelocityConstraint(goal_velocity, goal_velocity, 1.0);

// TODO: Add constraints on joint position/velocity/acceleration
// trajopt.AddPositionBounds(
// plant_->GetPositionLowerLimits(),
// plant_->GetPositionUpperLimits());
// trajopt.AddVelocityBounds(
// plant_->GetVelocityLowerLimits(),
// plant_->GetVelocityUpperLimits());
trajopt.AddPositionBounds(lower_position_bounds_eigen, upper_position_bounds_eigen);
trajopt.AddVelocityBounds(lower_velocity_bounds_eigen, upper_velocity_bounds_eigen);

// Add constraints on duration
// TODO: These should be parameters
Expand Down Expand Up @@ -181,9 +221,7 @@ void KTOptPlanningContext::setRobotDescription(std::string robot_description)
// TODO:(kamiradi) Figure out object parsing
// auto robot_instance = Parser(plant_, scene_graph_).AddModelsFromString(robot_description_, ".urdf");

// HACK: For now loading directly from drake's package map
const char* ModelUrl = "package://drake_models/franka_description/"
"urdf/panda_arm_hand.urdf";
const char* ModelUrl = params_.drake_robot_description.c_str();
const std::string urdf = PackageMap{}.ResolveUrl(ModelUrl);
auto robot_instance = Parser(&plant, &scene_graph).AddModels(urdf);
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("panda_link0"));
Expand Down
Loading