EXPERTISE / ROBOT SOFTWARE

Fast Motion Planning

Every second a robot spends thinking about how to move is a second it is not being productive. We build planners that find smooth, optimal, constraint-respecting motions fast enough to re-plan while the robot is on the move.

Explore the approach ↓
AI-generated illustration of a smiling engineer reviewing a robot motion-planning simulator.
01 / THE OPPORTUNITY

Turn motion planning into productive robot time

Faster cycles depend on more than moving the motors faster. A planner that understands the robot and its task can make better use of the motion the hardware can deliver. Three things make the difference:

  • Optimise for the job the robot has to do. A feasible path is a starting point. Cycle time, energy and smoothness need to be part of the objective if the motion is to improve throughput.
  • Put physical limits into the plan. Torque, payload, gripper attachment, obstacles and tool orientation shape which motions are useful. Modelling them together reduces the need for conservative corrections afterwards.
  • Plan at the pace of execution. A solution that arrives seconds later can miss the opportunity to respond. Fast optimisation and current scene information let the team work towards replanning while the robot moves.

That is an opportunity to improve cycle time without treating the robot's dynamics and task constraints as an afterthought.

02 / HOW WE SOLVE IT

Optimal control at planning speed

We have worked on motion planning since the OROCOS/KDL days and follow the field closely. Today our toolbox centres on fast optimal control:

five-stage motion-planning workflow in the shared expertise style
  1. Formulate the motion as an optimal control problem - time, energy or smoothness objective, with kinematics, full dynamics, collision and task constraints stated explicitly instead of hidden in heuristics.
  2. Solve it fast with Fatrop. We use Fatrop, an open-source, structure-exploiting interior-point solver for optimal control, and formulate the problem so it can exploit that structure. Intermodalics supports the open-source Fatrop development and employs its key contributor and maintainer. That expertise also helps make code generation of the dynamics tractable.
  3. Real-time 3D awareness. We combine the planner with real-time 3D reconstruction (e.g. NVIDIA Nvblox on Jetson) so collision constraints reflect the scene as it is now, not as it was mapped.
  4. Combine with the right global method where needed: sampling-based planners (OMPL), Nav2 for mobile bases, NURBS/smooth-curve planners for AGV load alignment, learned initial guesses, warm-starting.
  5. Integrate and prove it on your robot: ROS 2 / MoveIt 2 integration, real-time execution (our RTT heritage), and benchmarks against your current planner on your tasks.

The goal is motion that meets your cycle-time target while respecting the robot's physical limits - measured on your tasks, with the ability to replan when the scene changes.

03 / IN PRACTICE

In practice: an optimal 6-DOF motion in well under 100 ms

AI-generated illustration of engineers reviewing motion profiles beside a guarded industrial robot handling cartons.

Situation. A US warehouse-automation company needed its articulated robot arm to execute its approach and return motions as fast as physically possible - cycle time directly drives their key metric, picks per hour. Their trajectory-optimisation formulation, including joint-torque limits and constraints to keep the vacuum gripper from losing its load, was validated but took seconds to solve with IPOPT and SNOPT.

What we did. We integrated Fatrop with dynamics tooling such as Pinocchio and CasADi, restructured the problem formulation so the solver could exploit the trajectory-optimisation structure, made code generation of the dynamics feasible, and combined the planner with Nvblox for real-time 3D reconstruction of obstacles along the path.

Outcome. Collision-free, optimal 6-DOF motions planned in well under 100 ms, taking into account the robot's inertia, the payload and obstacles along the path - one to two orders of magnitude faster than before, and fast enough to re-plan online.

Also: The formulation accounted for the vacuum gripper's attachment limits, so planning included the constraints needed to retain the load.

04 / WHAT YOU GET

What you get

planning benchmark, integrated planner and team ownership
  • A benchmark of your current planner vs. an optimal-control formulation on your tasks (cycle time, smoothness, success rate).
  • A planner implementation integrated in your stack (ROS 2 / MoveIt 2 / Nav2 / custom), real-time capable where required.
  • Constraint modelling for your dynamics, safety and process requirements - and code generation that compiles.
  • Support and training by the people behind Fatrop, so your engineers own the formulation.

Engagement: a fixed-scope evaluation (typically 4–6 weeks) → development → support.

05 / FURTHER READING

Learn more from our blog

Real-Time Robotics: Before you start

Robotics systems rely heavily on sensors to provide accurate information about the environment and the robot itself. However, sensors are not perfect and often suffer from inaccuracies due to various factors. In this first Part, we dive into getting the basics right.

  • real-time
  • controls
  • kalman-filter
Read more
06 / YOUR NEXT STEP

"How much faster could your robot move? Let's benchmark it." Request a planning benchmark