Function roboplan::expandContinuousJointPositions

Function Documentation

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

Expands a joint position vector’s continuous joints from downstream algorithms.

That is, positions that are expressed as [theta] will be expanded to [cos(theta), sin(theta)].

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 expanded position vectors if successful, else a string describing the error.