core_motion_planning

class ServerManager

Manage a pool of typed Server instances for concurrent motion planning.

Callers acquire a server via WaitObtainAvailableServer() (blocks until one is free or the timeout expires) and must return it via ReturnServer() to prevent pool exhaustion. Obstacle and ACM state can be pushed to all servers via UpdateServers(). Thread-safe.

Public Functions

bool Init(ServerManagerInit &serverManagerInit)

Initialize the manager and populate server pools from the given parameters.

Parameters:: serverManagerInit – Fully populated initialization struct including all server pools.
Returns:: true if initialization succeeded.

bool AddServers(std::vector<std::unique_ptr<Server>> &servers)

Add multiple servers to the appropriate pools based on their runtime type.

Parameters:: servers – Vector of servers to add; ownership is transferred.
Returns:: true if all servers were added successfully.

bool AddServer(std::unique_ptr<Server> &server)

Add a single server to the appropriate pool based on its runtime type.

Parameters:: server – Server to add; ownership is transferred.
Returns:: true if the server was added successfully.

void UpdateServers(const std::map<std::string, core_types::CollisionObject> &obstaclesMap, const std::map<std::string, core_types::CollisionObject> &graspablesMap, const std::map<std::string, std::vector<std::string>> &acmMap)

Push updated world state to all managed servers.

Parameters:

obstaclesMap – Map of obstacle names to collision objects.
graspablesMap – Map of graspable object names to collision objects.
acmMap – Allowed collision matrix mapping link names to allowed collision partners.

std::unique_ptr<Server> WaitObtainAvailableServer(const ServerType serverType)

Acquire an available server of the given type, blocking until one is free or timeout expires.

Parameters:: serverType – Type of server to acquire.
Returns:: Unique pointer to the acquired Server, or nullptr on timeout.

std::unique_ptr<Server> WaitObtainAvailableServer(const ServerType serverType, const std::map<std::string, core_types::CollisionObject> &obstaclesMap, const std::map<std::string, core_types::CollisionObject> &graspablesMap, const std::map<std::string, std::vector<std::string>> &acmMap)

Acquire an available server and update its world state atomically.

Parameters:

serverType – Type of server to acquire.
obstaclesMap – Obstacle collision objects to apply before returning the server.
graspablesMap – Graspable collision objects to apply before returning the server.
acmMap – Allowed collision matrix to apply before returning the server.

Returns:

Unique pointer to the acquired and updated Server, or nullptr on timeout.

bool ReturnServer(std::unique_ptr<Server> &server)

Return a previously acquired server to the pool.

Parameters:: server – Server to return; must have been obtained via WaitObtainAvailableServer().
Returns:: true if the server was returned successfully.

class KinematicsSolver

Wrap core_arm_planning::KinematicsSolver to provide FK, FK-velocity, and IK services.

Non-copyable and move-only. Supports two Init overloads: one with a collision manager for collision-aware IK, and one without for FK-only use cases.

Public Functions

KinematicsSolver(): Default constructor.

~KinematicsSolver(): Destructor.

KinematicsSolver(KinematicsSolver&&) noexcept = default: Move constructor.

KinematicsSolver &operator=(KinematicsSolver&&) noexcept = default: Move assignment operator.

KinematicsSolver(const KinematicsSolver&) = delete: Deleted copy constructor — not copyable.

KinematicsSolver &operator=(const KinematicsSolver&) = delete: Deleted copy assignment — not copyable.

bool Init(const nlohmann::json &config, const KDL::Chain &armKDLChain, const core_types::JointLimit_t &armJointLimit, const std::shared_ptr<core_robot::Robot> robot, const std::shared_ptr<core_arm_planning::CollisionManager> collisionManager, const std::string &pathToOPWParams = "")

Initialize the solver with a collision manager for collision-aware IK.

Parameters:

config – JSON configuration for the underlying arm planner.
armKDLChain – KDL kinematic chain for the arm.
armJointLimit – Joint position limits for the arm.
robot – Shared robot model.
collisionManager – Collision manager used during IK to filter colliding solutions.
pathToOPWParams – Path to OPW parameter file; empty string disables OPW.

Returns:

true if initialization succeeded.

bool Init(const nlohmann::json &config, const KDL::Chain &armKDLChain, const core_types::JointLimit_t &armJointLimit, const std::shared_ptr<core_robot::Robot> robot, const std::string &pathToOPWParams = "")

Initialize the solver without a collision manager (FK-only use).

Parameters:

config – JSON configuration for the underlying arm planner.
armKDLChain – KDL kinematic chain for the arm.
armJointLimit – Joint position limits for the arm.
robot – Shared robot model.
pathToOPWParams – Path to OPW parameter file; empty string disables OPW.

Returns:

true if initialization succeeded.

double GetManipulability(const core_types::JointState_t &jointState, const core_types::RealVectorConstraint_t &jointPositionLimit)

Compute the manipulability index for the given joint state.

Parameters:

jointState – Joint configuration to evaluate.
jointPositionLimit – Joint position constraint used in the manipulability calculation.

Returns:

Manipulability scalar value (higher is more dexterous).

int DoFK(core_types::Pose_d &outputPose, const core_types::JointState_t &jointState, const std::string &tipName, const core_types::Pose_d &basePose = core_types::Pose_d())

Compute the forward kinematics pose for the given joint state.

Parameters:

outputPose – Output Cartesian pose of the tip link.
jointState – Input joint configuration.
tipName – Name of the tip link to evaluate FK for.
basePose – Optional base frame pose (default: identity).

Returns:

0 on success, non-zero error code on failure.

int DoFKVel(core_types::Twist_t &cartVel, const core_types::JointState_t &jointState, const std::string &tipName, const std::vector<double> &jointVel)

Compute the Cartesian velocity twist for the given joint state and velocities.

Parameters:

cartVel – Output Cartesian velocity twist at the tip link.
jointState – Current joint configuration.
tipName – Name of the tip link to evaluate FK velocity for.
jointVel – Joint velocity vector.

Returns:

0 on success, non-zero error code on failure.

int DoIK(std::vector<core_types::JointState_t> &goalStates, const core_types::TransformStamped_t &goalTF, const core_types::JointState_t &referenceJS, const motion_planning_types::IKConfig &ikConfig = motion_planning_types::IKConfig(), const core_types::Pose_d &basePose = core_types::Pose_d(), const core_types::RealVectorConstraint<double> &jointPositionConstraint = core_types::RealVectorConstraint<double>())

Compute inverse kinematics solutions for the given goal transform.

Parameters:

goalStates – Output vector of valid joint state solutions.
goalTF – Target end-effector transform.
referenceJS – Reference joint state used to seed and rank IK solutions.
ikConfig – IK solver configuration (default: library defaults).
basePose – Optional base frame pose (default: identity).
jointPositionConstraint – Optional joint position constraint to filter solutions.

Returns:

0 on success, non-zero error code on failure.

class MotionServer : public core_motion_planning::CollisionServer 

Provide full motion planning (path planning + parameterization) as a pooled server.

Extends CollisionServer with a PathPlanner and a Parameterizer. Callers acquire this server from ServerManager when they need end-to-end motion planning including collision-aware trajectory generation and time parameterization. Overrides UpdateImpl to propagate world state changes to the embedded PathPlanner.

Public Functions

bool Init(const CollisionServerInit &collisionServerInit, const KDL::Chain &armKdlChain, const core_types::JointLimit_t &armJointLimit, const std::string pathToOpwParams = "", const motion_planning_types::InverseDynamicsConfigStamped &idCfgStamped = motion_planning_types::InverseDynamicsConfigStamped())

Initialize the motion server with collision, kinematics, and dynamics parameters.

Parameters:

collisionServerInit – Initialization struct for the underlying collision server.
armKdlChain – KDL kinematic chain for the arm.
armJointLimit – Joint position limits for the arm.
pathToOpwParams – Path to OPW parameter file; empty string disables OPW.
idCfgStamped – Inverse dynamics configuration; default-constructed value disables dynamics.

Returns:

true if initialization succeeded.

Public Members

std::unique_ptr<PathPlanner> pathPlanner: Path planner for collision-aware joint-space and Cartesian planning.

std::unique_ptr<Parameterizer> parameterizer: Trajectory parameterizer for time-optimal motion profiles.

class IKServer : public core_motion_planning::CollisionServer 

Provide inverse kinematics computation with integrated collision checking as a pooled server.

Extends CollisionServer with a KinematicsSolver. Callers acquire this server from ServerManager when they need IK solutions validated against the collision scene.

Public Functions

bool Init(const CollisionServerInit &collisionServerInit, const KDL::Chain &armKdlChain, const core_types::JointLimit_t &armJointLimit, const std::string pathToOpwParams = "")

Initialize the IK server with collision and kinematics parameters.

Parameters:

collisionServerInit – Initialization struct for the underlying collision server.
armKdlChain – KDL kinematic chain for the arm.
armJointLimit – Joint position limits for the arm.
pathToOpwParams – Path to OPW parameter file; empty string disables OPW.

Returns:

true if initialization succeeded.

Public Members

std::unique_ptr<KinematicsSolver> kinematicsSolver: Underlying kinematics solver for IK queries.

class FKServer : public core_motion_planning::Server

Provide forward kinematics computation as a pooled server.

Owns a KinematicsSolver initialized for FK queries. Intended to be acquired from ServerManager by callers that need FK without collision checking.

Public Functions

bool Init(const FKServerInit &fkServerInit)

Initialize the FK server from the given parameters.

Parameters:: fkServerInit – Fully populated initialization struct.
Returns:: true if initialization succeeded.

Public Members

std::unique_ptr<KinematicsSolver> kinematicsSolver: Underlying kinematics solver for FK queries.

class ParameterizerServer : public core_motion_planning::CollisionServer 

Provide trajectory parameterization with integrated collision checking as a pooled server.

Extends CollisionServer with a Parameterizer. Callers acquire this server from ServerManager when they need time-optimal parameterization of pre-planned paths with optional inverse dynamics support.

Public Functions

bool Init(const CollisionServerInit &collisionServerInit, const core_types::JointLimit_t &armJointLimit, const motion_planning_types::InverseDynamicsConfigStamped &idCfgStamped = motion_planning_types::InverseDynamicsConfigStamped())

Initialize the parameterizer server with collision and dynamics parameters.

Parameters:

collisionServerInit – Initialization struct for the underlying collision server.
armJointLimit – Joint position, velocity, and acceleration limits for the arm.
idCfgStamped – Inverse dynamics configuration; default-constructed value disables dynamics.

Returns:

true if initialization succeeded.

Public Members

std::unique_ptr<Parameterizer> parameterizer: Trajectory parameterizer for time-optimal motion profiles.

class CollisionServer : public core_motion_planning::Server

Provide collision checking as a pooled server.

Manages a robot model and collision manager. Supports quick (broadphase) and full (narrowphase with contact info) collision checks, as well as dynamic world updates. Intended to be acquired from ServerManager.

Subclassed by core_motion_planning::IKServer, core_motion_planning::MotionServer, core_motion_planning::ParameterizerServer

Public Functions

bool Init(const CollisionServerInit &collisionServerInit)

Initialize the collision server from the given parameters.

Parameters:: collisionServerInit – Fully populated initialization struct.
Returns:: true if initialization succeeded.

void Update(const std::map<std::string, core_types::CollisionObject> &obstaclesMap, const std::map<std::string, core_types::CollisionObject> &graspablesMap, const std::map<std::string, std::vector<std::string>> &acmMap)

Update the world state with new obstacle, graspable, and ACM data.

Parameters:

obstaclesMap – Map of obstacle names to collision objects.
graspablesMap – Map of graspable object names to collision objects.
acmMap – Allowed collision matrix mapping link names to allowed partners.

bool AppendCollisionObjects(const motion_planning_types::AppendCollisionConcept &appendCollisionConcept)

Append additional collision objects to the collision scene.

Parameters:: appendCollisionConcept – Descriptor of objects to append.
Returns:: true if the objects were appended successfully.

bool QuickCheckCollision(const core_types::JointState_t &jointState, const core_types::Pose_d &basePose = core_types::Pose_d())

Perform a fast broadphase collision check for the given joint state.

Parameters:

jointState – Joint configuration to check.
basePose – Optional base frame pose (default: identity).

Returns:

true if the configuration is collision-free.

bool CheckCollisionWithPrintIfFail(const core_types::JointState_t &jointState, const core_types::Pose_d &basePose = core_types::Pose_d())

Check collision and print diagnostic information if a collision is detected.

Parameters:

jointState – Joint configuration to check.
basePose – Optional base frame pose (default: identity).

Returns:

true if the configuration is collision-free.

bool FullCheckCollision(std::vector<core_types::ContactInfo> &contactInfos, const core_types::JointState_t &jointState, const core_types::Pose_d &basePose = core_types::Pose_d(), bool doPrint = false)

Perform a full narrowphase collision check and collect contact information.

Parameters:

contactInfos – Output vector of contact details populated on collision.
jointState – Joint configuration to check.
basePose – Optional base frame pose (default: identity).
doPrint – If true, print collision information to the log.

Returns:

true if the configuration is collision-free.

class PathPlanner

Plan collision-free joint-space and Cartesian paths for a robot arm.

Wraps core_arm_planning::PathPlanner and core_arm_planning::JointCouplingPathPlanner. Non-copyable and move-only. Supports joint-space planning, Cartesian interpolation, joint-coupled planning, world updates, constraint checking, and goal conversion.

Public Functions

PathPlanner(): Default constructor.

~PathPlanner(): Destructor.

PathPlanner(PathPlanner&&) noexcept = default: Move constructor.

PathPlanner &operator=(PathPlanner&&) noexcept = default: Move assignment operator.

PathPlanner(const PathPlanner&) = delete: Deleted copy constructor — not copyable.

PathPlanner &operator=(const PathPlanner&) = delete: Deleted copy assignment — not copyable.

bool Init(const nlohmann::json &config, const std::map<std::string, core_types::CollisionObject> &obstaclesMap, const std::map<std::string, core_types::CollisionObject> &graspablesMap, const KDL::Chain &armKDLChain, const core_types::JointLimit_t &armJointLimit, const std::shared_ptr<core_robot::Robot> robot, const std::shared_ptr<KinematicsSolver> kinematicsSolver, const std::shared_ptr<core_arm_planning::CollisionManager> collisionManager, const std::string &pathToOPWParams = "")

Initialize the path planner with robot, kinematics, and world state.

Parameters:

config – JSON configuration for the underlying arm planner.
obstaclesMap – Initial obstacle collision objects.
graspablesMap – Initial graspable collision objects.
armKDLChain – KDL kinematic chain for the arm.
armJointLimit – Joint position limits for the arm.
robot – Shared robot model.
kinematicsSolver – Shared kinematics solver for IK during planning.
collisionManager – Shared collision manager for collision checking.
pathToOPWParams – Path to OPW parameter file; empty string disables OPW.

Returns:

true if initialization succeeded.

int DoJointPlan(core_types::JointTrajectory_t &outputPath, core_types::JointState_t &outputState, const core_types::JointState_t &startState, const std::vector<core_types::JointState_t> &goalStates, const JointPlanConfig &jointPlanConfig = JointPlanConfig(), const core_types::RealVectorConstraint_t &jointPositionConstraint = core_types::RealVectorConstraint_t())

Plan a collision-free joint-space path from start to one of the goal states.

Parameters:

outputPath – Output joint trajectory (positions only).
outputState – Output final joint state reached.
startState – Starting joint configuration.
goalStates – Candidate goal joint configurations; planner selects one.
jointPlanConfig – Planning configuration (default: library defaults).
jointPositionConstraint – Optional joint position constraint to enforce during planning.

Returns:

0 on success, non-zero error code on failure.

int DoCartesianInterpolate(core_types::JointTrajectory_t &outputPath, core_types::JointState_t &outputState, const core_types::JointState_t &startState, const std::vector<core_types::JointState_t> &goalStates, const std::string &tipName, const CartesianInterpolateConfig &cartesianConfig = CartesianInterpolateConfig())

Interpolate a Cartesian path between start and goal states.

Parameters:

outputPath – Output joint trajectory along the interpolated path.
outputState – Output final joint state reached.
startState – Starting joint configuration.
goalStates – Candidate goal joint configurations.
tipName – Name of the tip link to interpolate in Cartesian space.
cartesianConfig – Cartesian interpolation configuration (default: library defaults).

Returns:

0 on success, non-zero error code on failure.

int DoJointCoupledPlan(core_types::JointTrajectory_t &outputPath, core_types::JointState_t &outputState, const core_types::JointState_t &startState, const std::vector<core_types::JointState_t> &goalStates, const JointCoupledPlanConfig &jointCoupledPlanConfig = JointCoupledPlanConfig(), const core_types::RealVectorConstraint_t &jointPositionConstraint = core_types::RealVectorConstraint<double>())

Plan a path using joint coupling constraints between arm joints.

Parameters:

outputPath – Output joint trajectory.
outputState – Output final joint state reached.
startState – Starting joint configuration.
goalStates – Candidate goal joint configurations.
jointCoupledPlanConfig – Joint coupling configuration (default: library defaults).
jointPositionConstraint – Optional joint position constraint.

Returns:

0 on success, non-zero error code on failure.

void UpdateWorld(const std::map<std::string, core_types::CollisionObject> &obstaclesMap, const std::map<std::string, core_types::CollisionObject> &graspablesMap, const std::map<std::string, std::vector<std::string>> &acm)

Update the planner’s world state with new obstacle and ACM data.

Parameters:

obstaclesMap – Map of obstacle names to collision objects.
graspablesMap – Map of graspable object names to collision objects.
acm – Allowed collision matrix.

int CheckConstraints(const core_types::JointState_t &startState, const core_types::JointState_t &goalState, const core_types::msg::Constraints_t &constraints)

Check whether a motion from start to goal satisfies the given constraints.

Parameters:

startState – Starting joint configuration.
goalState – Goal joint configuration.
constraints – Motion constraints to validate against.

Returns:

0 if constraints are satisfied, non-zero error code otherwise.

int GoalConversion(std::vector<core_types::JointState_t> &goalStates, const motion_planning_types::Goal &goal, const core_types::JointState_t &referenceJS, const motion_planning_types::IKConfig &ikConfig = motion_planning_types::IKConfig())

Convert a high-level Goal to a set of joint state candidates via IK.

Parameters:

goalStates – Output vector of joint state candidates.
goal – High-level goal descriptor (joint or Cartesian).
referenceJS – Reference joint state used to seed and rank IK solutions.
ikConfig – IK solver configuration (default: library defaults).

Returns:

0 on success, non-zero error code on failure.