core_arm_planning

class PathPlanner

Plan joint-space paths using OMPL with integrated collision checking.

Wraps an OMPL planner (RRTConnect, RRTstar, or KPIECE1), applies joint-limit and collision validity checks, and retries with random seeds when the primary plan fails. Also supports Cartesian-space interpolation and path splicing.

Subclassed by core_arm_planning::JointCouplingPathPlanner

Public Functions

PathPlanner(const nlohmann::json &config, const std::shared_ptr<core_robot::Robot> &robot, const KDL::Chain &arm_kdl_chain, const core_types::JointLimit_t &arm_joint_limit, const std::map<std::string, core_types::CollisionObject> &obstacles_map, const std::map<std::string, core_types::CollisionObject> &graspables_map, const std::string path_to_opw_params = "")

Construct and initialize the planner from config and robot model.

Parameters:

  • config – JSON configuration (planner type, timeout, retries, etc.).

  • robot – Shared robot model providing collision geometry.

  • arm_kdl_chain – KDL kinematic chain for the planning arm.

  • arm_joint_limit – Joint position limits for the arm.

  • obstacles_map – Named collision objects acting as static obstacles.

  • graspables_map – Named collision objects currently held by the gripper.

  • path_to_opw_params – Path to an OPW parameters file; empty string disables OPW IK.

bool CheckAngularTolerance(const double &r1, const double &r2, const double &t)

Check whether two angular values are within a given tolerance.

Parameters:

  • r1 – First angle (radians).

  • r2 – Second angle (radians).

  • t – Tolerance (radians).

Returns:

true if |r1 - r2| <= t.

PlanningResult PlanInJointSpace(const core_types::JointStated_t &startState, const core_types::JointStated_t &goalState, const core_types::msg::Constraints_t &constraint, const core_types::Pose_d &basePose, const core_types::JointStated_t &endEffectorState, core_types::JointTrajectory_t &jointPath, const core_types::JointLimit_t &newLimit)

Plan a joint-space path from start to goal under the given constraints.

Parameters:

  • startState – Starting joint configuration.

  • goalState – Target joint configuration.

  • constraint – Motion constraints to enforce during planning.

  • basePose – Robot base pose in the world frame.

  • endEffectorState – Fixed end-effector joint state (e.g., gripper).

  • jointPath – Output trajectory populated on success.

  • newLimit – Effective joint limits to apply for this plan.

Returns:

PlanningResult indicating success or the reason for failure.

bool InterpolateInCartesianSpace(const core_types::JointStated_t &startState, const core_types::JointStated_t &goalState, const core_types::Pose_d &basePose, core_types::JointTrajectory_t &jointPath, core_types::JointStated_t &resGoalState, const unsigned int &nMinPoints = 3, const bool allowPathRepairing = false, const bool checkObstacleCollision = true)

Interpolate a collision-free path between two joint states via Cartesian space.

Parameters:

  • startState – Starting joint configuration.

  • goalState – Target joint configuration.

  • basePose – Robot base pose in the world frame.

  • jointPath – Output joint trajectory.

  • resGoalState – Resolved goal joint state (may differ from goalState).

  • nMinPoints – Minimum number of waypoints in the output path.

  • allowPathRepairing – If true, attempt to repair segments that fail IK.

  • checkObstacleCollision – If true, reject waypoints in collision with obstacles.

Returns:

true if a complete interpolated path was produced.

bool ToolMotionToJointMotion(const core_types::JointStated_t &startState, const std::vector<core_types::Twist_t> &twistsWrtArm, const std::function<bool(const core_types::JointStated_t&)> &isStateValid, core_types::JointTrajectory_t &jointPath, core_types::JointStated_t &endState)

Convert a sequence of Cartesian tool twists into a joint-space trajectory.

Parameters:

  • startState – Starting joint configuration.

  • twistsWrtArm – Ordered list of tool twists expressed in the arm frame.

  • isStateValid – Caller-supplied validity predicate for each candidate joint state.

  • jointPath – Output joint trajectory.

  • endState – Joint state reached after applying all twists.

Returns:

true if all twists were successfully converted.

bool MergePaths(const std::vector<core_types::JointTrajectory_t> &trajs, core_types::JointTrajectory_t &mergedTraj)

Merge a list of trajectories into a single concatenated trajectory.

Parameters:

  • trajs – Ordered list of trajectories to merge.

  • mergedTraj – Output merged trajectory.

Returns:

true if the merge succeeded without conflicts.

bool RepairPath(const core_types::JointTrajectory_t &path, core_types::JointTrajectory_t &repairedPath)

Repair a joint trajectory by replanning segments that are invalid.

Parameters:

  • path – Input trajectory that may contain invalid waypoints.

  • repairedPath – Output trajectory with invalid segments replaced.

Returns:

true if the path was successfully repaired.

PlanningResult PlanSplicedPath(const core_types::JointTrajectory_t &fromPath, const core_types::JointTrajectory_t &toPath, const core_types::Pose_d &basePose, core_types::JointTrajectory_t &plannedPath)

Plan a path that connects two paths or points smoothly.

Generates a collision-free joint-space segment bridging the tail of fromPath to the head of toPath.

Parameters:

  • fromPath – Source trajectory (may be a single-point path).

  • toPath – Destination trajectory (may be a single-point path).

  • basePose – Robot base pose in the world frame.

  • plannedPath – Output bridging trajectory.

Returns:

PlanningResult indicating success or the reason for failure.

PlanningResult PlanSplicedPath(const core_types::JointStated_t &fromState, const core_types::JointTrajectory_t &toPath, const core_types::Pose_d &basePose, core_types::JointTrajectory_t &plannedPath)

Plan a spliced path from a single start state to a trajectory.

Parameters:

  • fromState – Single start joint configuration.

  • toPath – Destination trajectory (may be a single-point path).

  • basePose – Robot base pose in the world frame.

  • plannedPath – Output bridging trajectory.

Returns:

PlanningResult indicating success or the reason for failure.

PlanningResult PlanSplicedPath(const core_types::JointTrajectory_t &fromPath, const core_types::JointStated_t &toState, const core_types::Pose_d &basePose, core_types::JointTrajectory_t &plannedPath)

Plan a spliced path from a trajectory to a single goal state.

Parameters:

  • fromPath – Source trajectory (may be a single-point path).

  • toState – Single target joint configuration.

  • basePose – Robot base pose in the world frame.

  • plannedPath – Output bridging trajectory.

Returns:

PlanningResult indicating success or the reason for failure.

bool UpdateWorld(const std::map<std::string, core_types::CollisionObject> &obstaclesMap, const std::map<std::string, core_types::CollisionObject> &graspablesMap)

Update the collision world with new obstacles and graspables.

Parameters:

  • obstaclesMap – New set of named obstacle collision objects.

  • graspablesMap – New set of named graspable collision objects.

Returns:

true if all collision managers were updated successfully.

bool 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 collision world with new obstacles, graspables, and allowed collision matrix.

Parameters:

  • obstaclesMap – New set of named obstacle collision objects.

  • graspablesMap – New set of named graspable collision objects.

  • ACM – Allowed collision matrix mapping link names to exempt partner names.

Returns:

true if all collision managers were updated successfully.

void ConfigureOmplPathPlanner(std::shared_ptr<ompl::base::Planner> &omplPlanner)

Configure an OMPL planner instance with parameters from the JSON config.

Parameters:

omplPlanner – Planner instance to configure (RRTstar, RRTConnect, or KPIECE1).

class KinematicsSolver

Solve inverse and forward kinematics using TRAC-IK and optional OPW analytic IK.

Provides FK via KDL recursive solvers and IK via TRAC-IK, with optional OPW analytic kinematics for compatible robot geometries (6-DOF or 5-DOF arms). Multiple DoIK overloads support single-solution and multi-solution queries with configurable solver type and seed state.

Public Functions

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

Construct the solver from config, robot model, and KDL chain.

Parameters:

  • config – JSON configuration (IK timeout, solver type, etc.).

  • robot – Shared robot model.

  • armKDLChain – KDL kinematic chain for the arm.

  • armJointLimit – Joint position limits for the arm.

  • pathToOPWParams – Path to an OPW parameter file; empty string disables OPW IK.

int DoIK(const core_types::Pose_d &pose, core_types::JointStated_t &goalState)

Compute IK for a target pose using the default solver and arm root.

Parameters:

  • pose – Target end-effector pose.

  • goalState – Output joint configuration on success.

Returns:

Number of solutions found (> 0 on success, <= 0 on failure).

int DoIK(const core_types::Pose_d &pose, const std::string &solverType, core_types::JointStated_t &goalState)

Compute IK for a target pose using the specified solver type.

Parameters:

  • pose – Target end-effector pose.

  • solverType – Solver identifier (e.g., “trac_ik”, “opw”).

  • goalState – Output joint configuration on success.

Returns:

Number of solutions found (> 0 on success, <= 0 on failure).

int DoIK(const core_types::Pose_d &pose, const std::string &solverType, const core_types::JointStated_t &startState, core_types::JointStated_t &goalState)

Compute IK for a target pose seeded from a given start state.

Parameters:

  • pose – Target end-effector pose.

  • solverType – Solver identifier.

  • startState – Seed joint configuration for the solver.

  • goalState – Output joint configuration on success.

Returns:

Number of solutions found (> 0 on success, <= 0 on failure).

int DoIK(const core_types::Pose_d &pose, std::vector<core_types::JointStated_t> &goalStates)

Compute multiple IK solutions for a target pose using the default solver.

Parameters:

  • pose – Target end-effector pose.

  • goalStates – Output list of joint configurations.

Returns:

Number of solutions found.

int DoIK(const core_types::Pose_d &pose, const std::string &solverType, std::vector<core_types::JointStated_t> &goalStates)

Compute multiple IK solutions using the specified solver type.

Parameters:

  • pose – Target end-effector pose.

  • solverType – Solver identifier.

  • goalStates – Output list of joint configurations.

Returns:

Number of solutions found.

int DoIK(const core_types::Pose_d &pose, const std::string &solverType, const core_types::JointStated_t &startState, std::vector<core_types::JointStated_t> &goalStates)

Compute multiple IK solutions seeded from a given start state.

Parameters:

  • pose – Target end-effector pose.

  • solverType – Solver identifier.

  • startState – Seed joint configuration for the solver.

  • goalStates – Output list of joint configurations.

Returns:

Number of solutions found.

int DoIK(const core_types::Pose_d &pose, const core_types::Pose_d &basePose, const std::string &solverType, const std::string &tipName, const core_types::JointStated_t &startState, std::vector<core_types::JointStated_t> &goalStates)

Compute multiple IK solutions for a specific tip frame in the robot model.

Parameters:

  • pose – Target tip-frame pose (in world coordinates).

  • basePose – Robot base pose in the world frame.

  • solverType – Solver identifier.

  • tipName – Name of the tip link to solve for.

  • startState – Seed joint configuration for the solver.

  • goalStates – Output list of joint configurations.

Returns:

Number of solutions found.

void FilterValidGoalStates(std::vector<core_types::JointStated_t> &goalStates)

Remove goal states that violate joint limits or other validity criteria.

Parameters:

goalStates – Joint configuration list modified in-place to remove invalid entries.

int DoIKVel(const KDL::Twist &cartVel, const core_types::JointStated_t &startState, std::vector<double> &jointVel)

Compute joint velocities from a Cartesian velocity using velocity IK.

Parameters:

  • cartVel – Desired Cartesian velocity twist.

  • startState – Current joint configuration.

  • jointVel – Output joint velocity vector.

Returns:

Number of solutions found (> 0 on success, <= 0 on failure).

bool DoFK(const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose, const std::string &tipName, core_types::Pose_d &pose)

Compute the forward kinematics pose for a named tip link.

Parameters:

  • jointState – Current joint configuration.

  • basePose – Robot base pose in the world frame.

  • tipName – Name of the tip link to evaluate.

  • pose – Output pose of the tip link in the world frame.

Returns:

true if FK was computed successfully.

bool DoArmFK(const core_types::JointStated_t &jointStates, core_types::Pose_d &pose)

Compute the forward kinematics pose of the arm end-effector.

Parameters:

  • jointStates – Current joint configuration.

  • pose – Output end-effector pose in the arm root frame.

Returns:

true if FK was computed successfully.

int DoFKVel(const core_types::JointStated_t &state, const std::vector<double> &jointVelocity, const std::string &tipName, KDL::Twist &cartVelocity)

Compute the Cartesian velocity of a tip link from joint velocities.

Parameters:

  • state – Current joint configuration.

  • jointVelocity – Joint velocity vector.

  • tipName – Name of the tip link to evaluate.

  • cartVelocity – Output Cartesian velocity twist.

Returns:

Number of FK velocity evaluations performed (> 0 on success).

double GetManipulability(const core_types::JointStated_t &jointState, const core_types::JointLimit_t &jointLimit, const std::string &solveType)

Compute the manipulability measure at a given joint state.

Parameters:

  • jointState – Current joint configuration.

  • jointLimit – Joint position limits used for normalization.

  • solveType – Solver type string used to select the Jacobian computation.

Returns:

Manipulability scalar (higher values indicate better dexterity).

double GetManipulation2Manipulability(const core_types::JointStated_t &jointState, const core_types::JointLimit_t &jointLimit)

Compute the second manipulability measure (manipulability-to-manipulability ratio).

Parameters:

  • jointState – Current joint configuration.

  • jointLimit – Joint position limits used for normalization.

Returns:

Manipulability-to-manipulability scalar.

bool InverseDynamics(const core_types::JointStated_t &states)

Compute inverse dynamics for the given joint states.

Parameters:

states – Current joint configuration (positions, velocities, and accelerations).

Returns:

true if inverse dynamics was computed successfully. // TODO(doc): clarify whether velocities/accelerations are embedded in JointStated_t or assumed zero.

class CollisionManager

Manage FCL-based collision checking for a robot arm in a dynamic world.

Maintains FCL broad-phase managers for self-collision, obstacle collision, and graspable-object collision. Supports both quick boolean checks and full contact-detail queries. The allowed collision matrix (ACM) can be updated at runtime to exempt specific link pairs from collision reporting.

Public Functions

CollisionManager(const nlohmann::json &config, const std::shared_ptr<core_robot::Robot> &robot, const std::map<std::string, core_types::CollisionObject> &obstaclesMap, const std::map<std::string, core_types::CollisionObject> &graspablesMap)

Construct and initialize the collision manager.

Parameters:

  • config – JSON configuration (contact limits, etc.).

  • robot – Shared robot model providing link geometry.

  • obstaclesMap – Named static obstacle collision objects.

  • graspablesMap – Named graspable collision objects attached to the gripper.

void UpdateRobotTF(const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose)

Update the robot’s forward-kinematics transforms for all links.

Parameters:

  • jointState – Current joint configuration of the robot.

  • basePose – Robot base pose in the world frame.

bool QuickCheckSelfCollision(const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose)

Quick check the self collision.

Parameters:

  • jointState – The joint state of the robot.

  • basePose – The base pose of the robot.

Returns:

True if the self collision is detected, false otherwise.

bool QuickCheckGraspablesCollision(const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose)

Quick check the graspables collision.

Parameters:

  • jointState – The joint state of the robot.

  • basePose – The base pose of the robot.

Returns:

True if the graspables collision is detected, false otherwise.

bool QuickCheckObstaclesCollision(const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose)

Quick check the obstacles collision.

Parameters:

  • jointState – The joint state of the robot.

  • basePose – The base pose of the robot.

Returns:

True if the obstacles collision is detected, false otherwise.

bool QuickCheckCollision(const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose)

Quick check the collision between robot and world.

Parameters:

  • jointState – The joint state of the robot.

  • basePose – The base pose of the robot.

Returns:

True if the collision is detected, false otherwise.

bool QuickCheckCollision(const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose, const std::string &group1, const std::string &group2)

Quick check the collision between two groups.

Parameters:

  • jointState – The joint state of the robot.

  • basePose – The base pose of the robot.

  • group1 – The first group.

  • group2 – The second group.

Returns:

True if the collision is detected, false otherwise.

bool QuickCheckCollision(const std::string &group1, const std::string &group2)

Quick check the collision between two groups.

Parameters:

  • group1 – The first group.

  • group2 – The second group.

Returns:

True if the collision is detected, false otherwise.

void FullCheckCollision(const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose, const std::string &group1, const std::string &group2, bool enableContact, bool &isCollision, int &numCollision, contact_map_t &contactMap, std::vector<core_types::ContactInfo> &contacts)

Perform a full collision check between two named object groups and populate contact details.

Parameters:

  • jointState – Current joint configuration.

  • basePose – Robot base pose in the world frame.

  • group1 – Name of the first collision group.

  • group2 – Name of the second collision group.

  • enableContact – If true, compute and populate contact geometry.

  • isCollision – Output flag set to true if any collision was detected.

  • numCollision – Output count of detected collision contacts.

  • contactMap – Output map of colliding object-pair names to contact descriptions.

  • contacts – Output list of detailed contact information structs.

bool FullCheckCollision(std::vector<core_types::ContactInfo> &contacts, const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose, bool enableContact = false, bool doPrint = false)

Perform a full collision check across all group pairs and return contact details.

Parameters:

  • contacts – Output list of detailed contact information structs.

  • jointState – Current joint configuration.

  • basePose – Robot base pose in the world frame.

  • enableContact – If true, compute and populate contact geometry.

  • doPrint – If true, log collision details to the console.

Returns:

true if the configuration is collision-free.

bool FullCheckAndPrintCollision(const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose)

Perform a full collision check and print any detected collisions to the console.

Parameters:

  • jointState – Current joint configuration.

  • basePose – Robot base pose in the world frame.

Returns:

true if the configuration is collision-free.

void UpdateACM(const std::map<std::string, std::vector<std::string>> &ACM)

Update the allowed collision matrix.

Parameters:

ACM – The allowed collision matrix.

void UpdateObstacles(const std::map<std::string, core_types::CollisionObject> &obstacles)

Update the obstacles map.

Parameters:

obstacles – The obstacles.

void UpdateGraspables(const std::map<std::string, core_types::CollisionObject> &graspables)

Update the graspables map.

Parameters:

graspables – The graspables.

enum class core_arm_planning::PlanningResult

Enumerate result codes for a path planning request.

Values:

enumerator SUCCESS

A valid, collision-free path was found.

enumerator INVALID_START

Start state is in collision or violates joint limits.

enumerator INVALID_GOAL

Goal state is in collision or violates joint limits.

enumerator TIMEOUT

Retry budget exhausted before a solution was found.

enumerator NO_SOLUTION

Planner searched the full budget but could not connect start to goal.