Class Scene

Class Documentation

class Scene

Primary scene representation for planning and control.

Public Functions

Scene(const std::string &name, const std::filesystem::path &urdf_path, const std::filesystem::path &srdf_path, const std::vector<std::filesystem::path> &package_paths = std::vector<std::filesystem::path>(), const std::filesystem::path &yaml_config_path = std::filesystem::path())

Basic constructor.

Parameters:

  • name – The name of the scene.

  • urdf_path – Path to the URDF file.

  • srdf_path – Path to the SRDF file.

  • package_paths – A vector of package paths to look for packages.

  • yaml_config_path – Path to the YAML configuration file with additional information.

Scene(const std::string &name, const std::string &urdf, const std::string &srdf, const std::vector<std::filesystem::path> &package_paths = std::vector<std::filesystem::path>(), const std::filesystem::path &yaml_config_path = std::filesystem::path())

Basic constructor with pre-parsed URDF and SRDF options.

Parameters:

  • name – The name of the scene.

  • urdf – XML String of the URDF.

  • srdf – XML String of the SRDF.

  • package_paths – A vector of package paths to look for packages.

  • yaml_config_path – Path to the YAML configuration file with additional information.

inline const std::string &getName() const

Gets the scene’s name.

Returns:

The scene name.

inline const pinocchio::Model &getModel() const

Gets the scene’s internal Pinocchio model.

Returns:

The Pinocchio model.

inline const pinocchio::Data &getData() const

Gets the scene’s internal Pinocchio data (read-only).

Returns:

The Pinocchio data.

inline const std::vector<std::string> &getJointNames() const

Gets the scene’s full joint names, including mimic joints.

Returns:

A vector of joint names.

inline const std::vector<std::string> &getActuatedJointNames() const

Gets the scene’s actuated (non-mimic) joint names.

Returns:

A vector of joint names.

tl::expected<JointInfo, std::string> getJointInfo(const std::string &joint_name) const

Gets the information for a specific joint.

Parameters:

joint_name – The name of the joint.

Returns:

The joint information struct if successful, else a string describing the error.

double configurationDistance(const Eigen::VectorXd &q_start, const Eigen::VectorXd &q_end) const

Gets the distance between two joint configurations.

Parameters:

  • q_start – The starting joint positions.

  • q_end – The ending joint positions.

Returns:

The configuration-space distance between the two positions.

void setRngSeed(unsigned int seed)

Sets the seed for the random number generator (RNG).

Parameters:

seed – The seed to set.

Eigen::VectorXd randomPositions()

Generates random positions for the robot model.

Returns:

The random positions.

std::optional<Eigen::VectorXd> randomCollisionFreePositions(size_t max_samples = 1000)

Generates random collision-free positions for the robot model.

Parameters:

max_tries – The maximum number of samples to attempt.

Returns:

The random positions, if successful, else std::nullopt.

bool hasCollisions(const Eigen::VectorXd &q, const bool debug = false) const

Checks collisions at specified joint positions.

Parameters:

  • q – The joint positions.

  • debug – If true, prints debug information and does not stop at first collision. This parameter is disabled by default.

Returns:

True if there are collisions, else false.

bool isValidPose(const Eigen::VectorXd &q) const

Checks if the specified joint positions are valid with respect to joint limits.

Parameters:

q – The joint positions.

Returns:

True if the positions respect joint limits, else false.

void applyMimics(Eigen::VectorXd &q) const

Applies mimic joint relationships to a position vector.

Parameters:

q – The joint positions.

Eigen::VectorXd toFullJointPositions(const std::string &group_name, const Eigen::VectorXd &q) const

Converts partial joint positions to full joint positions.

This includes adding new joints and applying mimic relationships.

Parameters:

  • group_name – The name of the joint group.

  • q – The original (partial) joint positions.

Returns:

The full joint positions.

Eigen::VectorXd interpolate(const Eigen::VectorXd &q_start, const Eigen::VectorXd &q_end, const double fraction) const

Interpolates between two joint configurations.

Parameters:

  • q_start – The starting joint configuration.

  • q_end – The ending joint configuration.

  • fraction – The interpolation coefficient, between 0 and 1.

Eigen::VectorXd integrate(const Eigen::VectorXd &q, const Eigen::VectorXd &v) const

Integrates a velocity vector from a configuration using Lie group operations.

Parameters:

  • q – The starting joint configuration (size model.nq).

  • v – The velocity / displacement vector to integrate (size model.nv).

Returns:

The resulting joint configuration after integration.

Eigen::Matrix4d forwardKinematics(const Eigen::VectorXd &q, const std::string &frame_name) const

Calculates forward kinematics for a specific frame.

Parameters:

  • q – The joint configuration.

  • frame_name – The name of the frame for which to perform forward kinematics.

Returns:

The 4x4 matrix denoting the transform of the specified frame.

void computeFrameJacobian(const Eigen::VectorXd &q, pinocchio::FrameIndex frame_id, pinocchio::ReferenceFrame reference_frame, Eigen::Ref<Eigen::MatrixXd> jacobian) const

Computes the frame Jacobian for a specific frame.

Parameters:

  • q – The joint configuration.

  • frame_id – The Pinocchio frame ID.

  • reference_frame – The reference frame for the Jacobian (LOCAL or WORLD).

  • jacobian – Output matrix to store the Jacobian (must be pre-allocated to 6 x nv).

tl::expected<pinocchio::FrameIndex, std::string> getFrameId(const std::string &name) const

Get the Pinocchio model ID of a frame by its name.

Parameters:

name – The name of the frame to look up.

Returns:

The Pinocchio frame ID if successful, else a string describing the error.

tl::expected<JointGroupInfo, std::string> getJointGroupInfo(const std::string &name) const

Get the joint group information of a scene by its name.

Parameters:

name – The name of the joint group to look up.

Returns:

The joint group information if successful, else a string describing the error.

inline const Eigen::VectorXd &getCurrentJointPositions() const

Get the current joint positions for the full robot state.

Returns:

The current joint position vector.

inline void setJointPositions(const Eigen::VectorXd &positions)

Set the joint positions for the full robot state.

Returns:

The desired joint position vector.

Eigen::VectorXi getJointPositionIndices(const std::vector<std::string> &joint_names) const

Get the joint position indices for a set of joint names.

Parameters:

joint_names – The joint names for which to look up position indices.

Returns:

The corresponding joint position indices.

tl::expected<EigenVectorPair, std::string> getPositionLimitVectors(const std::string &group_name = "") const

Get the joint position limit vectors for a specified group.

Parameters:

group_name – The name of the group. Defaults to the complete robot model.

Returns:

A pair of vectors for the lower and upper joint position limits, if successful, or a string describing any errors.

tl::expected<EigenVectorPair, std::string> getVelocityLimitVectors(const std::string &group_name = "") const

Get the joint velocity limit vectors for a specified group.

Parameters:

group_name – The name of the group. Defaults to the complete robot model.

Returns:

A pair of vectors for the lower and upper joint velocity limits, if successful, or a string describing any errors.

tl::expected<EigenVectorPair, std::string> getAccelerationLimitVectors(const std::string &group_name = "") const

Get the joint acceleration limit vectors for a specified group.

Parameters:

group_name – The name of the group. Defaults to the complete robot model.

Returns:

A pair of vectors for the lower and upper joint acceleration limits, if successful, or a string describing any errors.

tl::expected<EigenVectorPair, std::string> getJerkLimitVectors(const std::string &group_name = "") const

Get the joint jerk limit vectors for a specified group.

Parameters:

group_name – The name of the group. Defaults to the complete robot model.

Returns:

A pair of vectors for the lower and upper joint jerk limits, if successful, or a string describing any errors.

tl::expected<void, std::string> addBoxGeometry(const std::string &name, const std::string &parent_frame, const Box &box, const Eigen::Matrix4d &tform, const Eigen::Vector4d &color)

Adds a box geometry to the scene.

Parameters:

  • name – The name of the object to add.

  • parent_frame – The name of the parent frame to add the object to.

  • box – The box geometry instance to add.

  • tform – The transform between the parent frame and the geometry.

  • color – The color of the geometry, in RGBA vector format.

Returns:

Void if successful, else a string describing the error.

tl::expected<void, std::string> addSphereGeometry(const std::string &name, const std::string &parent_frame, const Sphere &sphere, const Eigen::Matrix4d &tform, const Eigen::Vector4d &color)

Adds a sphere geometry to the scene.

Parameters:

  • name – The name of the object to add.

  • parent_frame – The name of the parent frame to add the object to.

  • sphere – The sphere geometry instance to add.

  • tform – The transform between the parent frame and the geometry.

  • color – The color of the geometry, in RGBA vector format.

Returns:

Void if successful, else a string describing the error.

tl::expected<void, std::string> addOcTreeGeometry(const std::string &name, const std::string &parent_frame, const OcTree &octree, const Eigen::Matrix4d &tform, const Eigen::Vector4d &color)

Adds a octree geometry to the scene.

Parameters:

  • name – The name of the object to add.

  • parent_frame – The name of the parent frame to add the object to.

  • octree – The octree geometry instance to add.

  • tform – The transform between the parent frame and the geometry.

  • color – The color of the geometry, in RGBA vector format.

Returns:

Void if successful, else a string describing the error.

tl::expected<void, std::string> addGeometry(const pinocchio::GeometryObject &geom_obj)

Adds a Pinocchio geometry object to the scene.

This can be made the sole public entrypoint to add a geometry once Pinocchio and Coal have working nanobind bindings compatible with this library.

Parameters:

geom_obj – The geometry object instance to add.

Returns:

Void if successful, else a string describing the error.

tl::expected<void, std::string> updateGeometryPlacement(const std::string &name, const std::string &parent_frame, Eigen::Matrix4d &tform)

Updates the placement of an object geometry in the scene.

Parameters:

  • name – The name of the object to update.

  • parent_frame – The parent frame of the transformation.

  • tform – The transform between the parent frame and the geometry.

tl::expected<void, std::string> removeGeometry(const std::string &name)

Removes a geometry from the scene.

Parameters:

name – The name of the object to remove.

tl::expected<std::vector<pinocchio::GeomIndex>, std::string> getCollisionGeometryIds(const std::string &body)

Gets a list of collision geometry IDs corresponding to a specified body.

The body name can either be a model frame name or a collision model geometry name.

Parameters:

body – The name of the body.

Returns:

A std::vector of collision geometry indices for the body if successful, else a string describing the error.

tl::expected<void, std::string> setCollisions(const std::string &body1, const std::string &body2, const bool enable)

Sets the allowable collisions for a pair of bodies in the model.

The body names can either be model frame names or collision model geometry names.

Parameters:

  • body1 – The name of the first body.

  • body2 – The name of the second body.

  • enable – If true, enables the collision; if false, disables it.

Returns:

Void if successful, else a string describing the error.

Friends

friend std::ostream &operator<<(std::ostream &os, const Scene &scene)

Prints basic information about the scene.