core_robot

class Robot

Represent a fully-described robot model including kinematics, joint limits, and collision geometry.

Robot owns the KDL tree and chain, joint limits, collision objects, mimic joints, coupled joint limits, and workspace constraints. The model may be populated either from a URDF file plus a JSON parameter blob (Init()) or from a pre-built RobotInit struct (AnotherInit()).

Public Functions

Robot(): Construct a Robot with default-initialized members.

Robot(const Robot &src)

Construct a deep copy of another Robot instance.

Parameters:: src – Source Robot to copy from.

~Robot(): Destroy the Robot and release all owned resources.

inline const KDL::Tree &GetKdlTree() const noexcept

Return a const reference to the internal KDL kinematic tree.

Returns:: Const reference to the KDL::Tree.

inline const KDL::Chain &GetKdlChain() const noexcept

Return a const reference to the default KDL kinematic chain.

Returns:: Const reference to the KDL::Chain stored at construction time.

KDL::Chain GetKdlChain(const std::string &rootName, const std::string &tipName)

Extract and return a KDL chain between two named segments.

Parameters:

rootName – Name of the root segment of the chain.
tipName – Name of the tip segment of the chain.

Returns:

KDL::Chain spanning rootName to tipName.

bool GetKdlChain(const std::string &rootName, const std::string &tipName, KDL::Chain &chain)

Extract a KDL chain between two named segments into an output parameter.

Parameters:

rootName – Name of the root segment of the chain.
tipName – Name of the tip segment of the chain.
chain – Output parameter populated with the extracted chain.

Returns:

true if the chain was successfully extracted; false otherwise.

inline const core_types::JointLimit &GetHardLimit() const noexcept

Return a const reference to the hard joint limits for all joints.

Returns:: Const reference to the allJointLimit member.

core_types::JointLimit GetJointLimit() const

Compute and return effective joint limits, accounting for any active constraints.

Returns:: JointLimit representing the current effective limits.

inline const core_types::JointStated_t &GetJointState() const noexcept

Return a const reference to the current joint state.

Returns:: Const reference to the joint state vector.

inline std::map<std::string, core_types::CollisionObject> &GetCollisionObjects() noexcept

Return a mutable reference to the collision object map.

Returns:: Reference to the map of collision objects keyed by link name.

inline const std::map<std::string, core_types::CollisionObject> &GetCollisionObjects() const noexcept

Return a const reference to the collision object map.

Returns:: Const reference to the map of collision objects keyed by link name.

inline const std::map<std::string, core_types::Pose_d> &GetTf() const noexcept

Return a const reference to the transform map from the last UpdateTf() call.

Returns:: Const reference to the map of link poses keyed by link name.

inline const urdf::Model &GetUrdfModel() const noexcept

Return a const reference to the parsed URDF model.

Returns:: Const reference to the urdf::Model.

inline const std::map<std::string, std::vector<std::string>> &GetAllowedCollisionMatrix() const noexcept

Return a const reference to the allowed collision matrix.

Returns:: Const reference to the map of link names to their allowed collision partners.

void SetAllowedCollisionMatrix(const std::map<std::string, std::vector<std::string>> &allowedCollisionMatrix)

Replace the allowed collision matrix with the provided one.

Parameters:: allowedCollisionMatrix – New allowed collision matrix to apply.

bool AddAllowedCollision(const std::string &object_1, const std::string &object_2)

Register a bidirectional allowed collision pair between two objects.

Parameters:

object_1 – Name of the first collision object.
object_2 – Name of the second collision object.

Returns:

true if the pair was added successfully; false if it already existed or inputs are invalid.

bool RemoveAllowedCollision(const std::string &object_1, const std::string &object_2)

Remove a previously registered allowed collision pair between two objects.

Parameters:

object_1 – Name of the first collision object.
object_2 – Name of the second collision object.

Returns:

true if the pair was removed successfully; false if it did not exist.

std::map<std::string, core_types::CollisionObject> GetCollisionObjectsVisualCopy()

Return a copy of the collision objects with visual-only transforms applied.

Returns:: Map of collision objects keyed by link name, reflecting the visual configuration.

void UpdateTf(const core_types::JointStated_t &jointState, const core_types::Pose_d &basePose = core_types::Pose_d())

Update all link transforms given a joint state and optional base pose.

Parameters:

jointState – Joint state used to compute forward kinematics.
basePose – Optional pose of the robot base in the world frame; defaults to identity.

void UpdateTf(const std::vector<std::string> &jointNames, const std::vector<double> &jointPositions, const core_types::Pose_d &basePose = core_types::Pose_d())

Update all link transforms given joint names, positions, and an optional base pose.

Parameters:

jointNames – Ordered list of joint names corresponding to jointPositions.
jointPositions – Joint position values matching the jointNames order.
basePose – Optional pose of the robot base in the world frame; defaults to identity.

bool IsJointStateValid(const core_types::JointStated_t &activeJointState) const

Check whether all active joints in a joint state satisfy the hard joint limits.

Parameters:: activeJointState – Joint state containing only the active (non-fixed) joints.
Returns:: true if all joints are within their hard limits; false otherwise.

bool HasMimicJoint() const

Check whether the robot model contains any mimic joints.

Returns:: true if at least one mimic joint is configured; false otherwise.

bool IsMimicJointLimitValid(const core_types::JointStated_t &activeJointState) const

Check whether all mimic joints satisfy their derived joint limits.

Parameters:: activeJointState – Joint state of the active joints used to compute mimic positions.
Returns:: true if all mimic joint positions are within their limits; false otherwise.

bool HasCoupledJointLimit() const

Check whether the robot model contains a coupled joint limit constraint.

Returns:: true if a coupled joint limit is configured; false otherwise.

bool IsCoupledJointLimitValid(const core_types::JointStated_t &activeJointState) const

Check whether the joint state satisfies the coupled joint limit constraint.

Parameters:: activeJointState – Joint state of the active joints evaluated against the constraint.
Returns:: true if the linear coupled constraint is satisfied; false otherwise.

bool IsAllJointLimitValid(const core_types::JointStated_t &activeJointState) const

Check whether all joint limits (hard, mimic, and coupled) are satisfied.

Parameters:: activeJointState – Joint state of the active joints to validate.
Returns:: true if the joint state is valid under all limit types; false otherwise.

bool HasWorkspaceConstraints() const

Check whether the robot model has any workspace constraints configured.

Returns:: true if at least one workspace constraint is present; false otherwise.

core_types::JointStated_t SampleRandomJointState() const

Sample a random joint state uniformly within the hard joint limits.

Returns:: A randomly sampled joint state within the configured limits.

bool IsWorkspaceConstraintsValid(const core_types::JointStated_t &activeJointState, const core_types::Pose_d &basePose = core_types::Pose_d()) const

Check whether a given joint state satisfies all workspace constraints.

Parameters:

activeJointState – Joint state used to compute forward kinematics for constraint testing.
basePose – Optional base pose in the world frame; defaults to identity.

Returns:

true if all workspace constraints are satisfied; false otherwise.

bool IsWorkspaceConstraintsValid() const

Check whether the current internal joint state satisfies all workspace constraints.

Returns:: true if all workspace constraints are satisfied for the stored joint state; false otherwise.

bool Init(const nlohmann::json &paramsJson, const std::string &robotUrdfPath)

Initialize the robot model from a JSON parameter blob and a URDF file path.

Parameters:

paramsJson – JSON object containing robot configuration parameters.
robotUrdfPath – Filesystem path to the URDF description file.

Returns:

true if initialization succeeded; false otherwise.

bool AnotherInit(const RobotInit &robotInit)

Initialize the robot model from a pre-built RobotInit struct.

Parameters:: robotInit – Fully populated initialization struct.
Returns:: true if initialization succeeded; false otherwise.

class WorkspaceConstraint

Represent a volumetric constraint that bounds where a kinematic link may reach.

Supported volume shapes are BOX, SPHERE, and CYLINDER. The constraint is expressed in a user-specified root frame, and the link under test is identified by tipFrame. Call IsInConstraint() to test whether a given pose satisfies the constraint.

Public Types

enum class Type_t

Enumerate the supported constraint volume shapes.

Values:

enumerator UNKNOWN

enumerator BOX

enumerator SPHERE

enumerator CYLINDER

enum class CylinderDimensionsAccessor_t

Enumerate dimension indices for a cylinder-shaped constraint.

Use these indices to access the dimensions array when type is CYLINDER.

Values:

enumerator CYLINDER_HEIGHT

enumerator CYLINDER_RADIUS

enum class SphereDimensionsAccessor_t

Enumerate the dimension index for a sphere-shaped constraint.

Use this index to access the dimensions array when type is SPHERE.

Values:

enumerator SPHERE_RADIUS

enum BoxDimensionsAccessor_t

Enumerate dimension indices for a box-shaped constraint.

Use these indices to access the dimensions array when type is BOX.

Values:

enumerator BOX_X

enumerator BOX_Y

enumerator BOX_Z

Public Functions

bool IsInConstraint(const core_types::Pose_d &root_to_tip_pose) const

Test whether a pose lies within the constraint volume.

Parameters:: root_to_tip_pose – Pose of tipFrame expressed in rootFrame.
Returns:: true if the pose is inside the constraint volume; false otherwise.

Public Members

std::string rootFrame: Frame in which the constraint volume is expressed.

std::string tipFrame: The kinematic link whose pose is tested against the constraint.

Type_t type = Type_t::UNKNOWN : Active shape type for this constraint instance.

std::array<double, 3> dimensions = {}: Shape dimensions; interpretation depends on type.

struct RobotInit

Specify all parameters required to construct a Robot instance via AnotherInit().

Required fields must be fully populated before passing to AnotherInit(). Optional fields may be left at their default values when not applicable to the robot model.

Public Members

KDL::Tree kdlTree: KDL kinematic tree parsed from the URDF.

core_types::JointLimit_t allJointLimit: Hard joint limits for all joints.

std::map<std::string, core_types::CollisionObject> collisionObjects: Collision geometry keyed by link name.

std::map<std::string, std::vector<std::string>> allowedCollisionMatrix: Pairs of links that may collide without triggering a collision event.

std::map<std::string, MimicJoint> mimicsMap: Optional: mimic joint relationships keyed by mimic joint name.

std::optional<CoupledJointLimit> coupledJointLimit: Optional: linear coupling constraint between joints.

std::vector<WorkspaceConstraint> workspaceConstraints: Optional: volumetric workspace constraints for kinematic links.

struct MimicJoint

Represent a joint that mirrors the position of another joint via a linear mapping.

When the robot model contains mimic joints, each MimicJoint records the relationship between the mimic joint and its parent (“mimiced”) joint, including the scale factor and offset used to derive the mimic joint’s position.

Public Functions

MimicJoint() = default: Construct a default MimicJoint with zero multiplier and offset.

inline MimicJoint(std::string jointNameIn, std::string mimicedJointNameIn, double multiplierIn, double offsetIn)

Construct a MimicJoint with explicit relationship parameters.

Parameters:

jointNameIn – Name of this mimic joint.
mimicedJointNameIn – Name of the parent joint being mimicked.
multiplierIn – Scale factor applied to the parent joint position.
offsetIn – Constant offset added after scaling.

Public Members

std::string jointName: Name of this mimic joint.

std::string mimicedJointName: Name of the parent joint being mimicked.

double multiplier = {0.0}: Scale factor applied to the parent joint position.

double offset = {0.0}: Constant offset added after scaling.

int mimicingJointStateIdx = {-1}: Index of this joint in the joint state vector; -1 if unset.

int mimicJointLimitIdx = {-1}: Index of this joint in the joint limit array; -1 if unset.