Classes
class	CollisionCheck

class	ParameterDescriptorBuilder

struct	PIDConfig

class	PoseTracking

class	Servo

class	ServoCalcs

class	ServoNode

struct	ServoParameters

class	JoyToServoPub

class	PoseTrackingFixture

class	ServoFixture

Typedefs
using	PoseTrackingPtr = std::shared_ptr< PoseTracking >

using	ServoPtr = std::shared_ptr< Servo >

using	SetParameterCallbackType = std::function< rcl_interfaces::msg::SetParametersResult(const rclcpp::Parameter &)>

using	ServoServer = ServoNode

Enumerations
enum class	PoseTrackingStatusCode : int8_t { INVALID = -1 , SUCCESS = 0 , NO_RECENT_TARGET_POSE = 1 , NO_RECENT_END_EFFECTOR_POSE = 2 , STOP_REQUESTED = 3 }

enum class	ServoType { CARTESIAN_SPACE , JOINT_SPACE }

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

Functions
void	enforceVelocityLimits (const moveit::core::JointModelGroup *joint_model_group, const double publish_period, sensor_msgs::msg::JointState &joint_state, const double override_velocity_scaling_factor=0.0)
	Decrease robot position change and velocity, if needed, to satisfy joint velocity limits. More...

const std::unordered_map< PoseTrackingStatusCode, std::string >	POSE_TRACKING_STATUS_CODE_MAP ({ { PoseTrackingStatusCode::INVALID, "Invalid" }, { PoseTrackingStatusCode::SUCCESS, "Success" }, { PoseTrackingStatusCode::NO_RECENT_TARGET_POSE, "No recent target pose" }, { PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE, "No recent end effector pose" }, { PoseTrackingStatusCode::STOP_REQUESTED, "Stop requested" } })

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_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" }, { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" } })

bool	isNonZero (const geometry_msgs::msg::TwistStamped &msg)
	Helper function for detecting zeroed message. More...

bool	isNonZero (const control_msgs::msg::JointJog &msg)
	Helper function for detecting zeroed message. More...

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. More...

double	velocityScalingFactorForSingularity (const moveit::core::JointModelGroup *joint_model_group, const Eigen::VectorXd &commanded_twist, const Eigen::JacobiSVD< Eigen::MatrixXd > &svd, const Eigen::MatrixXd &pseudo_inverse, const double hard_stop_singularity_threshold, const double lower_singularity_threshold, const double leaving_singularity_threshold_multiplier, rclcpp::Clock &clock, moveit::core::RobotStatePtr &current_state, StatusCode &status)
	Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion. More...

	TEST_F (PoseTrackingFixture, OutgoingMsgTest)

	TEST_F (ServoFixture, SelfCollision)

	TEST_F (ServoFixture, ExternalCollision)

	TEST_F (ServoFixture, SendTwistStampedTest)

	TEST_F (ServoFixture, SendJointServoTest)

	TEST_F (ServoFixture, DynamicParameterTest)

Typedef Documentation

◆ PoseTrackingPtr

using moveit_servo::PoseTrackingPtr = typedef std::shared_ptr<PoseTracking>

Definition at line 194 of file pose_tracking.h.

◆ ServoPtr

using moveit_servo::ServoPtr = typedef std::shared_ptr<Servo>

Definition at line 106 of file servo.h.

◆ ServoServer

using moveit_servo::ServoServer = typedef ServoNode

Definition at line 49 of file servo_server.h.

◆ SetParameterCallbackType

using moveit_servo::SetParameterCallbackType = typedef std::function<rcl_interfaces::msg::SetParametersResult(const rclcpp::Parameter&)>

Definition at line 49 of file servo_parameters.h.

Enumeration Type Documentation

◆ PoseTrackingStatusCode

enum moveit_servo::PoseTrackingStatusCode : int8_t

strong

Enumerator
INVALID
SUCCESS
NO_RECENT_TARGET_POSE
NO_RECENT_END_EFFECTOR_POSE
STOP_REQUESTED

Definition at line 66 of file pose_tracking.h.

◆ ServoType

enum moveit_servo::ServoType

strong

Enumerator
CARTESIAN_SPACE
JOINT_SPACE

Definition at line 73 of file servo_calcs.h.

◆ StatusCode

enum moveit_servo::StatusCode : int8_t

strong

Enumerator
INVALID
NO_WARNING
DECELERATE_FOR_APPROACHING_SINGULARITY
HALT_FOR_SINGULARITY
DECELERATE_FOR_COLLISION
HALT_FOR_COLLISION
JOINT_BOUND
DECELERATE_FOR_LEAVING_SINGULARITY

Definition at line 46 of file status_codes.h.

Function Documentation

◆ 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.

Definition at line 64 of file utilities.cpp.

Here is the caller graph for this function:

◆ enforceVelocityLimits()

void moveit_servo::enforceVelocityLimits	(	const moveit::core::JointModelGroup *	joint_model_group,
		const double	publish_period,
		sensor_msgs::msg::JointState &	joint_state,
		const double	override_velocity_scaling_factor = `0.0`
	)

Decrease robot position change and velocity, if needed, to satisfy joint velocity limits.

Parameters

joint_model_group	Active joint group. Used to retrieve limits.
joint_state	The command that will go to the robot.
override_velocity_scaling_factor	Option if the user wants a constant override of the velocity scaling. a value greater than 0 will override the internal calculations done by getVelocityScalingFactor.

Definition at line 72 of file enforce_limits.cpp.

Here is the caller graph for this function:

◆ isNonZero() [1/2]

bool moveit_servo::isNonZero ( const control_msgs::msg::JointJog & msg )

Helper function for detecting zeroed message.

Definition at line 53 of file utilities.cpp.

◆ isNonZero() [2/2]

bool moveit_servo::isNonZero ( const geometry_msgs::msg::TwistStamped & msg )

Helper function for detecting zeroed message.

Definition at line 46 of file utilities.cpp.

Here is the caller graph for this function:

◆ POSE_TRACKING_STATUS_CODE_MAP()

const std::unordered_map<PoseTrackingStatusCode, std::string> moveit_servo::POSE_TRACKING_STATUS_CODE_MAP ( { { PoseTrackingStatusCode::INVALID, "Invalid" }, { PoseTrackingStatusCode::SUCCESS, "Success" }, { PoseTrackingStatusCode::NO_RECENT_TARGET_POSE, "No recent target pose" }, { PoseTrackingStatusCode::NO_RECENT_END_EFFECTOR_POSE, "No recent end effector pose" }, { PoseTrackingStatusCode::STOP_REQUESTED, "Stop requested" } } )

◆ 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_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" }, { StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY, "Moving away from a singularity, decelerating" } } )

◆ TEST_F() [1/6]

moveit_servo::TEST_F	(	PoseTrackingFixture	,
		OutgoingMsgTest
	)

Definition at line 156 of file pose_tracking_test.cpp.

◆ TEST_F() [2/6]

moveit_servo::TEST_F	(	ServoFixture	,
		DynamicParameterTest
	)

Definition at line 147 of file test_servo_interface.cpp.

◆ TEST_F() [3/6]

moveit_servo::TEST_F	(	ServoFixture	,
		ExternalCollision
	)

Definition at line 80 of file test_servo_collision.cpp.

◆ TEST_F() [4/6]

moveit_servo::TEST_F	(	ServoFixture	,
		SelfCollision
	)

Definition at line 46 of file test_servo_collision.cpp.

◆ TEST_F() [5/6]

moveit_servo::TEST_F	(	ServoFixture	,
		SendJointServoTest
	)

Definition at line 86 of file test_servo_interface.cpp.

◆ TEST_F() [6/6]

moveit_servo::TEST_F	(	ServoFixture	,
		SendTwistStampedTest
	)

Definition at line 45 of file test_servo_interface.cpp.

◆ velocityScalingFactorForSingularity()

double moveit_servo::velocityScalingFactorForSingularity	(	const moveit::core::JointModelGroup *	joint_model_group,
		const Eigen::VectorXd &	commanded_twist,
		const Eigen::JacobiSVD< Eigen::MatrixXd > &	svd,
		const Eigen::MatrixXd &	pseudo_inverse,
		const double	hard_stop_singularity_threshold,
		const double	lower_singularity_threshold,
		const double	leaving_singularity_threshold_multiplier,
		rclcpp::Clock &	clock,
		moveit::core::RobotStatePtr &	current_state,
		StatusCode &	status
	)

Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion.

Parameters

[in]	joint_model_group	The MoveIt group
[in]	commanded_twist	The commanded Cartesian twist
[in]	svd	A singular value decomposition of the Jacobian
[in]	pseudo_inverse	The pseudo-inverse of the Jacobian
[in]	hard_stop_singularity_threshold	Halt if condition(Jacobian) > hard_stop_singularity_threshold
[in]	lower_singularity_threshold	Decelerate if condition(Jacobian) > lower_singularity_threshold
[in]	leaving_singularity_threshold_multiplier	Allow faster motion away from singularity
[in,out]	clock	A ROS clock, for logging
[in,out]	current_state	The state of the robot. Used in internal calculations.
[out]	status	Singularity status

Definition at line 88 of file utilities.cpp.

Here is the caller graph for this function:

Classes

Typedefs

Enumerations

Functions

Typedef Documentation

◆ PoseTrackingPtr

◆ ServoPtr

◆ ServoServer

◆ SetParameterCallbackType

Enumeration Type Documentation

◆ PoseTrackingStatusCode

◆ ServoType

◆ StatusCode

Function Documentation

◆ convertIsometryToTransform()

◆ enforceVelocityLimits()

◆ isNonZero() [1/2]

◆ isNonZero() [2/2]

◆ POSE_TRACKING_STATUS_CODE_MAP()

◆ SERVO_STATUS_CODE_MAP()

◆ TEST_F() [1/6]

◆ TEST_F() [2/6]

◆ TEST_F() [3/6]

◆ TEST_F() [4/6]

◆ TEST_F() [5/6]

◆ TEST_F() [6/6]

◆ velocityScalingFactorForSingularity()