Function roboplan::collapseContinuousJointPositions

Function Documentation

tl::expected<Eigen::VectorXd, std::string> roboplan::collapseContinuousJointPositions(const Scene &scene, const std::string &group_name, const Eigen::VectorXd &q_orig)

Collapses a joint position vector’s continuous joints for downstream algorithms.

That is, positions that are expressed as [cos(theta), sin(theta)] will be collapsed to [theta], assuming being between +/- pi.

Parameters:

  • scene – The scene from which to look up joint information.

  • group_name – The name of the joint group corresponding to the position vector.

  • q_orig – The original position vectors.

Returns:

The collapsed position vectors if successful, else a string describing the error.