Robot

The Robot model class is a base class for storing the current state of the roboter. For example,it keeps track of the joint positions and recomputes the center of mass every time the robot moves. It also stores the footprint and mass of the robot.

The JointStateSubscriber in hector_math_ros can be used to keep the model in sync with an actual or simulated robot.

Example

auto robot_model = std::make_shared<hector_math::UrdfRobotModel<double>>( urdf_model );
hector_math::JointStateSubscriber sub( robot_model );
if ( !sub.waitForFullState( ros::Duration( 3 )) {
  // Didn't get the state for all joints within 3 seconds
}

API

template<typename Scalar> class RobotModel

Subclassed by UrdfRobotModel< Scalar >

Public Types

using Ptr = std::shared_ptr<RobotModel<Scalar>>

using ConstPtr = std::shared_ptr<const RobotModel<Scalar>>

Public Functions

inline RobotModel(std::vector<std::string> joint_names, std::vector<Scalar> joint_positions)

inline explicit RobotModel(std::unordered_map<std::string, Scalar> joint_states)

inline virtual void updateJointPositions(const std::unordered_map<std::string, Scalar> &positions): Updates the joint positions of the robot model with the given values for the corresponding joint name.

inline virtual void updateJointPositions(const std::vector<Scalar> &positions): Updates the joint positions of the robot model with the given values. Indices have to correspond to names in jointNames().

inline virtual void updateJointPosition(size_t index, Scalar position): Updates the joint position of a single joint of the robot model with the given value. The index corresponds to the names in jointNames().

inline const std::vector<std::string> &jointNames() const: The names of the joints represented in this robot model.

inline const std::vector<Scalar> &jointPositions() const

inline Scalar getJointPosition(const std::string &name) const

inline const Vector3<Scalar> &centerOfMass() const: The position of the center of mass in the robot coordinate frame.

virtual const Scalar &mass() const = 0

inline const Eigen::AlignedBox<Scalar, 3> &axisAlignedBoundingBox() const

inline const Polygon<Scalar> &footprint() const: The footprint is an approximated polygon of the robots hull projected to the ground.

template<typename Scalar> class RobotFootprint

Subclassed by UrdfRobotFootprint< Scalar >

Public Functions

virtual Scalar getMinimum(const GridMap<Scalar> &map, const Vector2<Scalar> &pos, Scalar orientation, Scalar maximum) const = 0

template<typename Scalar> class JointStateSubscriber

Convenience class to keep a RobotModel in sync with the actual joint positions as published on /joint_states

Public Functions

inline bool waitForFullState(const std::chrono::milliseconds &timeout = -1)

Wait for a full update of all joint positions. Usually, the joint states are broadcast by multiple publishers, hence, this method waits until it received an update for each joint position in the RobotModel. If the node clock hasn’t started yet, this method will first wait until it is before waiting the specified duration.

Parameters:

timeout – The maximum duration to wait before giving up. Pass -1 to wait indefinitely. Default: -1
spin – If true spin the ros main CallbackQueue while waiting to process messages, if you have your own spinner, pass false.

Returns:

True if a full update was received, false if not.

inline void pause(): Stops updating the joint states of the model but will continue collecting changes.

inline void resume(): Updates the joint positions of the model to the latest information and resumes continuously updating the model.