Classes
class	CollisionMonitor

struct	JointJogCommand

struct	KinematicState

struct	PoseCommand

class	Servo

class	ServoNode

struct	TwistCommand

Typedefs
typedef std::pair< StatusCode, Eigen::VectorXd >	JointDeltaResult

typedef std::variant< JointJogCommand, TwistCommand, PoseCommand >	ServoInput

typedef std::unordered_map< std::string, std::size_t >	JointNameToMoveGroupIndexMap

Enumerations
enum class	StatusCode : int8_t { INVALID = -1 , NO_WARNING = 0 , DECELERATE_FOR_APPROACHING_SINGULARITY = 1 , HALT_FOR_SINGULARITY = 2 , DECELERATE_FOR_LEAVING_SINGULARITY = 3 , DECELERATE_FOR_COLLISION = 4 , HALT_FOR_COLLISION = 5 , JOINT_BOUND = 6 }

enum class	CommandType : int8_t { JOINT_JOG = 0 , TWIST = 1 , POSE = 2 , MIN = JOINT_JOG , MAX = POSE }

Functions
JointDeltaResult	jointDeltaFromJointJog (const JointJogCommand &command, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const JointNameToMoveGroupIndexMap &joint_name_group_index_map)
	Compute the change in joint position for the given joint jog command.

JointDeltaResult	jointDeltaFromTwist (const TwistCommand &command, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const std::string &planning_frame, const JointNameToMoveGroupIndexMap &joint_name_group_index_map)
	Compute the change in joint position for the given twist command.

JointDeltaResult	jointDeltaFromPose (const PoseCommand &command, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const std::string &planning_frame, const std::string &ee_frame, const JointNameToMoveGroupIndexMap &joint_name_group_index_map)
	Compute the change in joint position for the given pose command.

JointDeltaResult	jointDeltaFromIK (const Eigen::VectorXd &cartesian_position_delta, const moveit::core::RobotStatePtr &robot_state, const servo::Params &servo_params, const JointNameToMoveGroupIndexMap &joint_name_group_index_map)
	Computes the required change in joint angles for given Cartesian change, using the robot's IK solver.

std::optional< std::string >	getIKSolverBaseFrame (const moveit::core::RobotStatePtr &robot_state, const std::string &group_name)
	Get the base frame of the current active joint group or subgroup's IK solver.

std::optional< std::string >	getIKSolverTipFrame (const moveit::core::RobotStatePtr &robot_state, const std::string &group_name)
	Get the tip (end-effector) frame of the current active joint group or subgroup's IK solver.

bool	isValidCommand (const Eigen::VectorXd &command)
	Checks if a given VectorXd is a valid command.

bool	isValidCommand (const Eigen::Isometry3d &command)
	Checks if a given Isometry3d (pose) is a valid command.

bool	isValidCommand (const TwistCommand &command)
	Checks if a given Twist command is valid.

bool	isValidCommand (const PoseCommand &command)
	Checks if a given Pose command is valid.

geometry_msgs::msg::Pose	poseFromCartesianDelta (const Eigen::VectorXd &delta_x, const Eigen::Isometry3d &base_to_tip_frame_transform)
	Create a pose message for the provided change in Cartesian position.

std::optional< trajectory_msgs::msg::JointTrajectory >	composeTrajectoryMessage (const servo::Params &servo_params, const std::deque< KinematicState > &joint_cmd_rolling_window)
	Create a trajectory message from a rolling window queue of joint state commands. Method optionally returns a trajectory message if one can be created.

void	updateSlidingWindow (KinematicState &next_joint_state, std::deque< KinematicState > &joint_cmd_rolling_window, double max_expected_latency, const rclcpp::Time &cur_time)
	Adds a new joint state command to a queue containing commands over a time window. Also modifies the velocities of the commands to help avoid overshooting.

std_msgs::msg::Float64MultiArray	composeMultiArrayMessage (const servo::Params &servo_params, const KinematicState &joint_state)
	Create a Float64MultiArray message from given joint state.

std::pair< double, StatusCode >	velocityScalingFactorForSingularity (const moveit::core::RobotStatePtr &robot_state, const Eigen::VectorXd &target_delta_x, const servo::Params &servo_params)
	Computes scaling factor for velocity when the robot is near a singularity.

double	jointLimitVelocityScalingFactor (const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, double scaling_override)
	Apply velocity scaling based on joint limits. If the robot model does not have velocity limits defined, then a scale factor of 1.0 will be returned.

std::vector< size_t >	jointVariablesToHalt (const Eigen::VectorXd &positions, const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, const std::vector< double > &margins)
	Finds the joint variable indices corresponding to joints exceeding allowable position limits.

geometry_msgs::msg::TransformStamped	convertIsometryToTransform (const Eigen::Isometry3d &eigen_tf, const std::string &parent_frame, const std::string &child_frame)
	Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped.

PoseCommand	poseFromPoseStamped (const geometry_msgs::msg::PoseStamped &msg)
	Convert a PoseStamped message to a Servo Pose.

planning_scene_monitor::PlanningSceneMonitorPtr	createPlanningSceneMonitor (const rclcpp::Node::SharedPtr &node, const servo::Params &servo_params)
	Creates the planning scene monitor used by servo.

KinematicState	extractRobotState (const moveit::core::RobotStatePtr &robot_state, const std::string &move_group_name)
	Extract the state from a RobotStatePtr instance.

const std::unordered_map< StatusCode, std::string >	SERVO_STATUS_CODE_MAP ({ { StatusCode::INVALID, "Invalid" }, { StatusCode::NO_WARNING, "No warnings" }, { StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY, "Moving closer to a singularity, decelerating" }, { StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" }, { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" }, { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } })

Variables
constexpr int	MIN_POINTS_FOR_TRAJ_MSG = 3

Typedef Documentation

◆ JointDeltaResult

typedef std::pair<StatusCode, Eigen::VectorXd> moveit_servo::JointDeltaResult

Definition at line 85 of file datatypes.hpp.

◆ JointNameToMoveGroupIndexMap

typedef std::unordered_map<std::string, std::size_t> moveit_servo::JointNameToMoveGroupIndexMap

Definition at line 134 of file datatypes.hpp.

◆ ServoInput

typedef std::variant<JointJogCommand, TwistCommand, PoseCommand> moveit_servo::ServoInput

Definition at line 111 of file datatypes.hpp.

Enumeration Type Documentation

◆ CommandType

enum class moveit_servo::CommandType : int8_t

strong

Enumerator
JOINT_JOG
TWIST
POSE
MIN
MAX

Definition at line 74 of file datatypes.hpp.

◆ StatusCode

enum class moveit_servo::StatusCode : int8_t

strong

Enumerator
INVALID
NO_WARNING
DECELERATE_FOR_APPROACHING_SINGULARITY
HALT_FOR_SINGULARITY
DECELERATE_FOR_LEAVING_SINGULARITY
DECELERATE_FOR_COLLISION
HALT_FOR_COLLISION
JOINT_BOUND

Definition at line 51 of file datatypes.hpp.

Function Documentation

◆ composeMultiArrayMessage()

std_msgs::msg::Float64MultiArray moveit_servo::composeMultiArrayMessage	(	const servo::Params &	servo_params,
		const KinematicState &	joint_state
	)

Create a Float64MultiArray message from given joint state.

Parameters

servo_params	The configuration used by servo, required for selecting position vs velocity.
joint_state	The joint state to be added into the Float64MultiArray.

Returns: The Float64MultiArray message.

Definition at line 258 of file common.cpp.

◆ composeTrajectoryMessage()

std::optional< trajectory_msgs::msg::JointTrajectory > moveit_servo::composeTrajectoryMessage	(	const servo::Params &	servo_params,
		const std::deque< KinematicState > &	joint_cmd_rolling_window
	)

Create a trajectory message from a rolling window queue of joint state commands. Method optionally returns a trajectory message if one can be created.

Parameters

servo_params	The configuration used by servo, required for setting some field of the trajectory message.
joint_cmd_rolling_window	A rolling window queue of joint state commands.

Returns: The trajectory message.

Definition at line 151 of file common.cpp.

Here is the caller graph for this function:

◆ convertIsometryToTransform()

geometry_msgs::msg::TransformStamped moveit_servo::convertIsometryToTransform	(	const Eigen::Isometry3d &	eigen_tf,
		const std::string &	parent_frame,
		const std::string &	child_frame
	)

Helper function for converting Eigen::Isometry3d to geometry_msgs/TransformStamped.

Parameters

eigen_tf	The isometry to be converted to TransformStamped.
parent_frame	The target frame.
child_frame	The current frame.

Returns: The isometry as a TransformStamped message.

Definition at line 454 of file common.cpp.

◆ createPlanningSceneMonitor()

planning_scene_monitor::PlanningSceneMonitorPtr moveit_servo::createPlanningSceneMonitor	(	const rclcpp::Node::SharedPtr &	node,
		const servo::Params &	servo_params
	)

Creates the planning scene monitor used by servo.

Definition at line 480 of file common.cpp.

Here is the caller graph for this function:

◆ extractRobotState()

KinematicState moveit_servo::extractRobotState	(	const moveit::core::RobotStatePtr &	robot_state,
		const std::string &	move_group_name
	)

Extract the state from a RobotStatePtr instance.

Parameters

robot_state	A RobotStatePtr instance.
move_group_name	The name of the planning group.

Returns: The state of the RobotStatePtr instance.

Definition at line 516 of file common.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getIKSolverBaseFrame()

std::optional< std::string > moveit_servo::getIKSolverBaseFrame	(	const moveit::core::RobotStatePtr &	robot_state,
		const std::string &	group_name
	)

Get the base frame of the current active joint group or subgroup's IK solver.

Parameters

robot_state	A pointer to the current robot state.
group_name	The currently active joint group name.

Returns: The IK solver base frame, if one exists, otherwise std::nullopt.

Definition at line 57 of file common.cpp.

◆ getIKSolverTipFrame()

std::optional< std::string > moveit_servo::getIKSolverTipFrame	(	const moveit::core::RobotStatePtr &	robot_state,
		const std::string &	group_name
	)

Get the tip (end-effector) frame of the current active joint group or subgroup's IK solver.

Parameters

robot_state	A pointer to the current robot state.
group_name	The currently active joint group name.

Returns: The IK solver tip frame, if one exists, otherwise std::nullopt.

Definition at line 72 of file common.cpp.

◆ isValidCommand() [1/4]

bool moveit_servo::isValidCommand ( const Eigen::Isometry3d & command )

Checks if a given Isometry3d (pose) is a valid command.

Parameters

command The command to be checked.

Returns: True if the command is valid, else False.

Definition at line 93 of file common.cpp.

Here is the call graph for this function:

◆ isValidCommand() [2/4]

bool moveit_servo::isValidCommand ( const Eigen::VectorXd & command )

Checks if a given VectorXd is a valid command.

Parameters

command The command to be checked.

Returns: True if the command is valid, else False.

Definition at line 87 of file common.cpp.

Here is the caller graph for this function:

◆ isValidCommand() [3/4]

bool moveit_servo::isValidCommand ( const PoseCommand & command )

Checks if a given Pose command is valid.

Parameters

command The command to be checked.

Returns: True if the command is valid, else False.

Definition at line 110 of file common.cpp.

Here is the call graph for this function:

◆ isValidCommand() [4/4]

bool moveit_servo::isValidCommand ( const TwistCommand & command )

Checks if a given Twist command is valid.

Parameters

command The command to be checked.

Returns: True if the command is valid, else False.

Definition at line 105 of file common.cpp.

Here is the call graph for this function:

◆ jointDeltaFromIK()

JointDeltaResult moveit_servo::jointDeltaFromIK	(	const Eigen::VectorXd &	cartesian_position_delta,
		const moveit::core::RobotStatePtr &	robot_state,
		const servo::Params &	servo_params,
		const JointNameToMoveGroupIndexMap &	joint_name_group_index_map
	)

Computes the required change in joint angles for given Cartesian change, using the robot's IK solver.

Parameters

cartesian_position_delta	The change in Cartesian position.
robot_state_	The current robot state as obtained from PlanningSceneMonitor.
servo_params	The servo parameters.
joint_name_group_index_map	Mapping between joint subgroup name and move group joint vector position.

Returns: The status and joint position change required (delta).

Definition at line 313 of file command.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ jointDeltaFromJointJog()

JointDeltaResult moveit_servo::jointDeltaFromJointJog	(	const JointJogCommand &	command,
		const moveit::core::RobotStatePtr &	robot_state,
		const servo::Params &	servo_params,
		const JointNameToMoveGroupIndexMap &	joint_name_group_index_map
	)

Compute the change in joint position for the given joint jog command.

Parameters

command	The joint jog command.
robot_state_	The current robot state as obtained from PlanningSceneMonitor.
servo_params	The servo parameters.
joint_name_group_index_map	Mapping between joint subgroup name and move group joint vector position.

Returns: The status and joint position change required (delta).

Definition at line 82 of file command.cpp.

Here is the call graph for this function:

◆ jointDeltaFromPose()

JointDeltaResult moveit_servo::jointDeltaFromPose	(	const PoseCommand &	command,
		const moveit::core::RobotStatePtr &	robot_state,
		const servo::Params &	servo_params,
		const std::string &	planning_frame,
		const std::string &	ee_frame,
		const JointNameToMoveGroupIndexMap &	joint_name_group_index_map
	)

Compute the change in joint position for the given pose command.

Parameters

command	The pose command.
robot_state_	The current robot state as obtained from PlanningSceneMonitor.
servo_params	The servo parameters.
planning_frame	The planning frame name.
ee_frame	The end effector frame name.
joint_name_group_index_map	Mapping between sub group joint name and move group joint vector position

Returns: The status and joint position change required (delta).

Definition at line 230 of file command.cpp.

Here is the call graph for this function:

◆ jointDeltaFromTwist()

JointDeltaResult moveit_servo::jointDeltaFromTwist	(	const TwistCommand &	command,
		const moveit::core::RobotStatePtr &	robot_state,
		const servo::Params &	servo_params,
		const std::string &	planning_frame,
		const JointNameToMoveGroupIndexMap &	joint_name_group_index_map
	)

Compute the change in joint position for the given twist command.

Parameters

command	The twist command.
robot_state_	The current robot state as obtained from PlanningSceneMonitor.
servo_params	The servo parameters.
planning_frame	The planning frame name.
joint_name_group_index_map	Mapping between joint subgroup name and move group joint vector position.

Returns: The status and joint position change required (delta).

Definition at line 147 of file command.cpp.

Here is the call graph for this function:

◆ jointLimitVelocityScalingFactor()

double moveit_servo::jointLimitVelocityScalingFactor	(	const Eigen::VectorXd &	velocities,
		const moveit::core::JointBoundsVector &	joint_bounds,
		double	scaling_override
	)

Apply velocity scaling based on joint limits. If the robot model does not have velocity limits defined, then a scale factor of 1.0 will be returned.

Parameters

velocities	The commanded velocities.
joint_bounds	The bounding information for the robot joints.
scaling_override	The user defined velocity scaling override.

Returns: The velocity scaling factor.

Definition at line 380 of file common.cpp.

Here is the caller graph for this function:

◆ jointVariablesToHalt()

std::vector< size_t > moveit_servo::jointVariablesToHalt	(	const Eigen::VectorXd &	positions,
		const Eigen::VectorXd &	velocities,
		const moveit::core::JointBoundsVector &	joint_bounds,
		const std::vector< double > &	margins
	)

Finds the joint variable indices corresponding to joints exceeding allowable position limits.

Parameters

positions	The joint positions.
velocities	The current commanded velocities.
joint_bounds	The allowable limits for the robot joints.
margins	Additional buffer on the actual joint limits.

Returns: The joint variable indices that violate the specified position limits.

Definition at line 411 of file common.cpp.

Here is the caller graph for this function:

◆ poseFromCartesianDelta()

geometry_msgs::msg::Pose moveit_servo::poseFromCartesianDelta	(	const Eigen::VectorXd &	delta_x,
		const Eigen::Isometry3d &	base_to_tip_frame_transform
	)

Create a pose message for the provided change in Cartesian position.

Parameters

delta_x	The change in Cartesian position.
base_to_tip_frame_transform	The transformation from robot base to ee frame.

Returns: The pose message.

Definition at line 115 of file common.cpp.

Here is the caller graph for this function:

◆ poseFromPoseStamped()

PoseCommand moveit_servo::poseFromPoseStamped ( const geometry_msgs::msg::PoseStamped & msg )

Convert a PoseStamped message to a Servo Pose.

Parameters

msg	The PoseStamped message.

Returns: The equivalent Servo Pose type.

Definition at line 464 of file common.cpp.

◆ SERVO_STATUS_CODE_MAP()

const std::unordered_map< StatusCode, std::string > moveit_servo::SERVO_STATUS_CODE_MAP ( { { StatusCode::INVALID, "Invalid" }, { StatusCode::NO_WARNING, "No warnings" }, { StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY, "Moving closer to a singularity, decelerating" }, { StatusCode::HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" }, { StatusCode::DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, { StatusCode::HALT_FOR_COLLISION, "Collision detected, emergency stop" }, { StatusCode::JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } } )

Here is the caller graph for this function:

◆ updateSlidingWindow()

void moveit_servo::updateSlidingWindow	(	KinematicState &	next_joint_state,
		std::deque< KinematicState > &	joint_cmd_rolling_window,
		double	max_expected_latency,
		const rclcpp::Time &	cur_time
	)

Adds a new joint state command to a queue containing commands over a time window. Also modifies the velocities of the commands to help avoid overshooting.

Parameters

next_joint_state	The next commanded joint state.
joint_cmd_rolling_window	Queue of containing a rolling window of joint commands.
max_expected_latency	The next_joint_state will be added to the joint_cmd_rolling_window with a time stamp of
cur_time	The current time stamp when the method is called. This value is used to update the time stamp of next_joint_state

Definition at line 203 of file common.cpp.

Here is the caller graph for this function:

◆ velocityScalingFactorForSingularity()

std::pair< double, StatusCode > moveit_servo::velocityScalingFactorForSingularity	(	const moveit::core::RobotStatePtr &	robot_state,
		const Eigen::VectorXd &	target_delta_x,
		const servo::Params &	servo_params
	)

Computes scaling factor for velocity when the robot is near a singularity.

Parameters

robot_state	A pointer to the current robot state.
target_delta_x	The vector containing the required change in Cartesian position.
servo_params	The servo parameters, contains the singularity thresholds.

Returns: The velocity scaling factor and the reason for scaling.

Definition at line 282 of file common.cpp.

Here is the caller graph for this function:

Variable Documentation

◆ MIN_POINTS_FOR_TRAJ_MSG

constexpr int moveit_servo::MIN_POINTS_FOR_TRAJ_MSG = 3

constexpr

Definition at line 58 of file common.hpp.

Classes

Typedefs

Enumerations

Functions

Variables

Typedef Documentation

◆ JointDeltaResult

◆ JointNameToMoveGroupIndexMap

◆ ServoInput

Enumeration Type Documentation

◆ CommandType

◆ StatusCode

Function Documentation

◆ composeMultiArrayMessage()

◆ composeTrajectoryMessage()

◆ convertIsometryToTransform()

◆ createPlanningSceneMonitor()

◆ extractRobotState()

◆ getIKSolverBaseFrame()

◆ getIKSolverTipFrame()

◆ isValidCommand() [1/4]

◆ isValidCommand() [2/4]

◆ isValidCommand() [3/4]

◆ isValidCommand() [4/4]

◆ jointDeltaFromIK()

◆ jointDeltaFromJointJog()

◆ jointDeltaFromPose()

◆ jointDeltaFromTwist()

◆ jointLimitVelocityScalingFactor()

◆ jointVariablesToHalt()

◆ poseFromCartesianDelta()

◆ poseFromPoseStamped()

◆ SERVO_STATUS_CODE_MAP()

◆ updateSlidingWindow()

◆ velocityScalingFactorForSingularity()

Variable Documentation

◆ MIN_POINTS_FOR_TRAJ_MSG