Class PFKinematics

Defined in File pf_kinematics.hpp

Class Documentation

class PFKinematics

Public Functions

PFKinematics() = default

explicit PFKinematics(const std::string &urdfFileName)

~PFKinematics() = default

inline pinocchio::Model &getModel()

void initializeCaches(const std::vector<std::string> &jointNames, const std::vector<std::string> &linkNames)

std::vector<Eigen::Affine3d> computeLinkTransforms(const std::vector<double> &jointPositions)

std::vector<PotentialFieldObstacle> buildObstaclesFromTransforms(const std::vector<Eigen::Affine3d> &transforms)

Using cached transforms vector aligned to collisionLinkNames, build obstacles using updated poses.

Parameters:: transforms – The new poses of collision links, aligned to collisionLinkNames
Returns:: std::vector<PotentialFieldObstacle> PotentialFieldObstacle objects with updated poses

std::vector<CollisionCatalogEntry> buildCollisionCatalog(const urdf::ModelInterface &model)

Builds the collision catalog from the URDF model, holding info about each collision object’s link, name, and Collision pointer.

Parameters:

model – The URDF model
repulsiveGain – The repulsive gain for the robot obstacles

Returns:

std::vector<CollisionCatalogEntry> The built collision catalog

PotentialFieldObstacle obstacleFromCollisionObject(const std::string &frameID, const urdf::Collision &collisionObject, const Eigen::Vector3d &position, const Eigen::Quaterniond &orientation)

Builds an Obstacle message from a URDF Collision object and given pose.

Parameters:

frameID – The frame ID for the obstacle’s pose to be defined in
collisionObject – The URDF Collision object, defining geometry and type
position – The position of the obstacle
orientation – The orientation of the obstacle

Returns:

PotentialFieldObstacle The constructed PFObstacle to be inserted in the PF

std::vector<PotentialFieldObstacle> updateObstaclesFromJointAngles(const std::vector<double> &jointAngles)

inline bool areCachesInitialized() const

inline const std::vector<std::string> &cachedJointNames() const

inline const std::vector<std::string> &cachedLinkNames() const

inline size_t getNumJoints() const

inline size_t getNumLinks() const

double estimateRobotExtentRadius()

Estimate a conservative bounding-sphere radius of the robot from its URDF using collision geometry. The radius is computed as the maximum of ||p_base|| + r_local across all collision objects, where p_base is the collision origin expressed in the base frame (at nominal zero joint values) and r_local is the local bounding-sphere radius of the collision geometry.

Note

Geometry handling:

Box(l,w,h): r_local = 0.5 * sqrt(l^2 + w^2 + h^2)
Sphere(r): r_local = r
Cylinder(r,len): r_local = sqrt(r^2 + (len/2)^2)
Mesh(file,scale): if mesh extents are unavailable, uses a conservative heuristic: r_local ≈ meshFallbackRadius * max(scale.x, scale.y, scale.z)

Returns:: double Estimated robot radius in meters. Returns 0.0 if no collision geometry is found or the URDF model is not initialized.

Eigen::MatrixXd getJacobianAtPoint(const std::string &linkName, const Eigen::Vector3d &pointInWorldFrame, const std::vector<double> &jointAngles)

Eigen::MatrixXd getSpatialJacobianAtPoint(const std::string &linkName, const Eigen::Vector3d &pointInWorldFrame, const std::vector<double> &jointAngles)

Computes the 6D Spatial Jacobian at a point attached to a link.

Parameters:

linkName – The name of the link
pointInWorldFrame – The point in world coordinates where the Jacobian is evaluated
jointAngles – The joint configuration

Returns:

Eigen::MatrixXd The 6xN Jacobian matrix [v; w]

SpatialVector computeEndEffectorPose(const std::vector<double> &jointAngles, const std::string &eeLinkName)

Computes the end-effector pose from joint angles using forward kinematics.

Parameters:

jointAngles – The joint configuration [rad]
eeLinkName – The name of the end-effector link

Returns:

SpatialVector The computed end-effector pose (position + orientation)

std::vector<double> computeInverseKinematics(const SpatialVector &targetPose, const std::vector<double> &seedJointAngles, const std::string &eeLinkName, int maxIterations = 100, double tolerance = 1e-4)

Computes inverse kinematics for the given target pose using a numerical solver (Newton-Raphson).

Parameters:

targetPose – The target end-effector pose.
seedJointAngles – The initial guess for joint angles.
eeLinkName – The name of the end-effector link.
maxIterations – Maximum number of iterations.
tolerance – Convergence tolerance (position and orientation).

Returns:

std::vector<double> The computed joint angles. Returns empty vector if failed.

double getEndEffectorMass(const std::string &eeLinkName, const double fallBackMass = 1.0) const

Try to get the mass of the end-effector link from the URDF model.

Parameters:

eeLinkName – The name of the end-effector link
fallBackMass – The mass to return if the link or inertial is not found or negligible (< 1 gram)

Returns:

double The mass of the end-effector link in kg, or the fallback mass if not found

Eigen::VectorXd jointValuesToVector(const std::vector<double> &jointValues)

Convert a vector of joint values to an Eigen vector.

Parameters:: jointValues – The joint values as std::vector<double>, either the joint positions or velocities
Returns:: Eigen::VectorXd The joint values as an Eigen vector

Eigen::MatrixXd getMassMatrix(const std::vector<double> &jointAngles, const double lambda = 1e-3)

Compute the Mass-Inertia Matrix at the given joint configuration.

Note

A lambda of 1e-3 was chosen empirically to balance numerical stability without distorting dynamics too much. If necessary, this value can be tuned based on how accurate the mass matrix needs to be

Parameters:

jointAngles – The joint configuration [rad]
lambda – Regularization parameter to ensure positive definiteness, default is 1e-3

Returns:

Eigen::MatrixXd The mass matrix

Eigen::VectorXd getCoriolisVector(const std::vector<double> &jointAngles, const std::vector<double> &jointVelocities)

Compute the Coriolis vector at the given joint configuration and velocities.

Parameters:

jointAngles – The joint configuration [rad]
jointVelocities – The joint velocities [rad/s]

Returns:

Eigen::VectorXd The Coriolis vector

Eigen::VectorXd getGravityVector(const std::vector<double> &jointAngles)

Compute the Gravity vector at the given joint configuration.

Parameters:: jointAngles – The joint configuration [rad]
Returns:: Eigen::VectorXd The Gravity vector