#include <pose_tracking.h>

Public Member Functions
bool	getCommandFrameTransform (geometry_msgs::TransformStamped &transform)

void	getPIDErrors (double &x_error, double &y_error, double &z_error, double &orientation_error)

PoseTrackingStatusCode	moveToPose (const Eigen::Vector3d &positional_tolerance, const double angular_tolerance, const double target_pose_timeout)

	PoseTracking (const ros::NodeHandle &nh, const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor)
	Constructor. Loads ROS parameters under the given namespace. More...

void	resetTargetPose ()
	Re-initialize the target pose to an empty message. Can be used to reset motion between waypoints. More...

void	stopMotion ()
	A method for a different thread to stop motion and return early from control loop. More...

void	updatePIDConfig (const double x_proportional_gain, const double x_integral_gain, const double x_derivative_gain, const double y_proportional_gain, const double y_integral_gain, const double y_derivative_gain, const double z_proportional_gain, const double z_integral_gain, const double z_derivative_gain, const double angular_proportional_gain, const double angular_integral_gain, const double angular_derivative_gain)
	Change PID parameters. Motion is stopped before the udpate. More...

Public Attributes
std::unique_ptr< moveit_servo::Servo >	servo_

Private Member Functions
geometry_msgs::TwistStampedConstPtr	calculateTwistCommand ()
	Use PID controllers to calculate a full spatial velocity toward a pose. More...

void	doPostMotionReset ()
	Reset flags and PID controllers after a motion completes. More...

bool	haveRecentEndEffectorPose (const double timeout)
	Return true if an end effector pose has been received within timeout [seconds]. More...

bool	haveRecentTargetPose (const double timeout)
	Return true if a target pose has been received within timeout [seconds]. More...

void	initializePID (const PIDConfig &pid_config, std::vector< control_toolbox::Pid > &pid_vector)
	Initialize a PID controller and add it to vector of controllers. More...

void	readROSParams ()
	Load ROS parameters for controller settings. More...

bool	satisfiesPoseTolerance (const Eigen::Vector3d &positional_tolerance, const double angular_tolerance)
	Check if XYZ, roll/pitch/yaw tolerances are satisfied. More...

void	targetPoseCallback (const geometry_msgs::PoseStampedConstPtr &msg)
	Subscribe to the target pose on this topic. More...

void	updateControllerSetpoints ()
	Update PID controller target positions & orientations. More...

void	updateControllerStateMeasurements ()
	Update PID controller states (positions & orientations) More...

Private Attributes
boost::optional< double >	angular_error_

PIDConfig	angular_pid_config_

std::vector< control_toolbox::Pid >	cartesian_orientation_pids_

std::vector< control_toolbox::Pid >	cartesian_position_pids_

Eigen::Isometry3d	command_frame_transform_

ros::Time	command_frame_transform_stamp_

const moveit::core::JointModelGroup *	joint_model_group_

ros::Rate	loop_rate_

std::string	move_group_name_

ros::NodeHandle	nh_

std::string	planning_frame_

planning_scene_monitor::PlanningSceneMonitorPtr	planning_scene_monitor_

robot_model::RobotModelConstPtr	robot_model_

std::atomic< bool >	stop_requested_

geometry_msgs::PoseStamped	target_pose_

std::mutex	target_pose_mtx_

ros::Subscriber	target_pose_sub_

tf2_ros::Buffer	transform_buffer_

tf2_ros::TransformListener	transform_listener_

ros::Publisher	twist_stamped_pub_

PIDConfig	x_pid_config_

PIDConfig	y_pid_config_

PIDConfig	z_pid_config_

Detailed Description

Class PoseTracking - subscribe to a target pose. Servo toward the target pose.

Definition at line 117 of file pose_tracking.h.

Constructor & Destructor Documentation

◆ PoseTracking()

moveit_servo::PoseTracking::PoseTracking	(	const ros::NodeHandle &	nh,
		const planning_scene_monitor::PlanningSceneMonitorPtr &	planning_scene_monitor
	)

Constructor. Loads ROS parameters under the given namespace.

Definition at line 46 of file pose_tracking.cpp.

Member Function Documentation

◆ calculateTwistCommand()

geometry_msgs::TwistStampedConstPtr moveit_servo::PoseTracking::calculateTwistCommand ( )

private

Use PID controllers to calculate a full spatial velocity toward a pose.

Definition at line 258 of file pose_tracking.cpp.

◆ doPostMotionReset()

void moveit_servo::PoseTracking::doPostMotionReset ( )

private

Reset flags and PID controllers after a motion completes.

Definition at line 320 of file pose_tracking.cpp.

◆ getCommandFrameTransform()

bool moveit_servo::PoseTracking::getCommandFrameTransform ( geometry_msgs::TransformStamped & transform )

Get the End Effector link transform. The transform from the MoveIt planning frame to EE link

Parameters

transform the transform that will be calculated

Returns: true if a valid transform was available

Definition at line 382 of file pose_tracking.cpp.

◆ getPIDErrors()

void moveit_servo::PoseTracking::getPIDErrors	(	double &	x_error,
		double &	y_error,
		double &	z_error,
		double &	orientation_error
	)

Definition at line 366 of file pose_tracking.cpp.

◆ haveRecentEndEffectorPose()

bool moveit_servo::PoseTracking::haveRecentEndEffectorPose ( const double timeout )

private

Return true if an end effector pose has been received within timeout [seconds].

Definition at line 213 of file pose_tracking.cpp.

◆ haveRecentTargetPose()

bool moveit_servo::PoseTracking::haveRecentTargetPose ( const double timeout )

private

Return true if a target pose has been received within timeout [seconds].

Definition at line 207 of file pose_tracking.cpp.

◆ initializePID()

void moveit_servo::PoseTracking::initializePID	(	const PIDConfig &	pid_config,
		std::vector< control_toolbox::Pid > &	pid_vector
	)

private

Initialize a PID controller and add it to vector of controllers.

Definition at line 200 of file pose_tracking.cpp.

◆ moveToPose()

PoseTrackingStatusCode moveit_servo::PoseTracking::moveToPose	(	const Eigen::Vector3d &	positional_tolerance,
		const double	angular_tolerance,
		const double	target_pose_timeout
	)

Definition at line 79 of file pose_tracking.cpp.

◆ readROSParams()

void moveit_servo::PoseTracking::readROSParams ( )

private

Load ROS parameters for controller settings.

Definition at line 144 of file pose_tracking.cpp.

◆ resetTargetPose()

void moveit_servo::PoseTracking::resetTargetPose ( )

Re-initialize the target pose to an empty message. Can be used to reset motion between waypoints.

Definition at line 375 of file pose_tracking.cpp.

◆ satisfiesPoseTolerance()

bool moveit_servo::PoseTracking::satisfiesPoseTolerance	(	const Eigen::Vector3d &	positional_tolerance,
		const double	angular_tolerance
	)

private

Check if XYZ, roll/pitch/yaw tolerances are satisfied.

Definition at line 218 of file pose_tracking.cpp.

◆ stopMotion()

void moveit_servo::PoseTracking::stopMotion ( )

A method for a different thread to stop motion and return early from control loop.

Definition at line 306 of file pose_tracking.cpp.

◆ targetPoseCallback()

void moveit_servo::PoseTracking::targetPoseCallback ( const geometry_msgs::PoseStampedConstPtr & msg )

private

Subscribe to the target pose on this topic.

Definition at line 233 of file pose_tracking.cpp.

◆ updateControllerSetpoints()

void moveit_servo::PoseTracking::updateControllerSetpoints ( )

private

Update PID controller target positions & orientations.

◆ updateControllerStateMeasurements()

void moveit_servo::PoseTracking::updateControllerStateMeasurements ( )

private

Update PID controller states (positions & orientations)

◆ updatePIDConfig()

void moveit_servo::PoseTracking::updatePIDConfig	(	const double	x_proportional_gain,
		const double	x_integral_gain,
		const double	x_derivative_gain,
		const double	y_proportional_gain,
		const double	y_integral_gain,
		const double	y_derivative_gain,
		const double	z_proportional_gain,
		const double	z_integral_gain,
		const double	z_derivative_gain,
		const double	angular_proportional_gain,
		const double	angular_integral_gain,
		const double	angular_derivative_gain
	)

Change PID parameters. Motion is stopped before the udpate.

Definition at line 333 of file pose_tracking.cpp.

Member Data Documentation

◆ angular_error_

boost::optional<double> moveit_servo::PoseTracking::angular_error_

private

Definition at line 220 of file pose_tracking.h.

◆ angular_pid_config_

PIDConfig moveit_servo::PoseTracking::angular_pid_config_

private

Definition at line 200 of file pose_tracking.h.

◆ cartesian_orientation_pids_

std::vector<control_toolbox::Pid> moveit_servo::PoseTracking::cartesian_orientation_pids_

private

Definition at line 198 of file pose_tracking.h.

◆ cartesian_position_pids_

std::vector<control_toolbox::Pid> moveit_servo::PoseTracking::cartesian_position_pids_

private

Definition at line 197 of file pose_tracking.h.

◆ command_frame_transform_

Eigen::Isometry3d moveit_servo::PoseTracking::command_frame_transform_

private

Definition at line 203 of file pose_tracking.h.

◆ command_frame_transform_stamp_

ros::Time moveit_servo::PoseTracking::command_frame_transform_stamp_

private

Definition at line 204 of file pose_tracking.h.

◆ joint_model_group_

const moveit::core::JointModelGroup* moveit_servo::PoseTracking::joint_model_group_

private

Definition at line 188 of file pose_tracking.h.

◆ loop_rate_

ros::Rate moveit_servo::PoseTracking::loop_rate_

private

Definition at line 192 of file pose_tracking.h.

◆ move_group_name_

std::string moveit_servo::PoseTracking::move_group_name_

private

Definition at line 190 of file pose_tracking.h.

◆ nh_

ros::NodeHandle moveit_servo::PoseTracking::nh_

private

Definition at line 184 of file pose_tracking.h.

◆ planning_frame_

std::string moveit_servo::PoseTracking::planning_frame_

private

Definition at line 215 of file pose_tracking.h.

◆ planning_scene_monitor_

planning_scene_monitor::PlanningSceneMonitorPtr moveit_servo::PoseTracking::planning_scene_monitor_

private

Definition at line 186 of file pose_tracking.h.

◆ robot_model_

robot_model::RobotModelConstPtr moveit_servo::PoseTracking::robot_model_

private

Definition at line 187 of file pose_tracking.h.

◆ servo_

std::unique_ptr<moveit_servo::Servo> moveit_servo::PoseTracking::servo_

Definition at line 151 of file pose_tracking.h.

◆ stop_requested_

std::atomic<bool> moveit_servo::PoseTracking::stop_requested_

private

Definition at line 218 of file pose_tracking.h.

◆ target_pose_

geometry_msgs::PoseStamped moveit_servo::PoseTracking::target_pose_

private

Definition at line 205 of file pose_tracking.h.

◆ target_pose_mtx_

std::mutex moveit_servo::PoseTracking::target_pose_mtx_

mutableprivate

Definition at line 206 of file pose_tracking.h.

◆ target_pose_sub_

ros::Subscriber moveit_servo::PoseTracking::target_pose_sub_

private

Definition at line 209 of file pose_tracking.h.

◆ transform_buffer_

tf2_ros::Buffer moveit_servo::PoseTracking::transform_buffer_

private

Definition at line 211 of file pose_tracking.h.

◆ transform_listener_

tf2_ros::TransformListener moveit_servo::PoseTracking::transform_listener_

private

Definition at line 212 of file pose_tracking.h.

◆ twist_stamped_pub_

ros::Publisher moveit_servo::PoseTracking::twist_stamped_pub_

private

Definition at line 195 of file pose_tracking.h.

◆ x_pid_config_

PIDConfig moveit_servo::PoseTracking::x_pid_config_

private

Definition at line 200 of file pose_tracking.h.

◆ y_pid_config_

PIDConfig moveit_servo::PoseTracking::y_pid_config_

private

Definition at line 200 of file pose_tracking.h.

◆ z_pid_config_

PIDConfig moveit_servo::PoseTracking::z_pid_config_

private

Definition at line 200 of file pose_tracking.h.

The documentation for this class was generated from the following files:

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PoseTracking()

Member Function Documentation

◆ calculateTwistCommand()

◆ doPostMotionReset()

◆ getCommandFrameTransform()

◆ getPIDErrors()

◆ haveRecentEndEffectorPose()

◆ haveRecentTargetPose()

◆ initializePID()

◆ moveToPose()

◆ readROSParams()

◆ resetTargetPose()

◆ satisfiesPoseTolerance()

◆ stopMotion()

◆ targetPoseCallback()

◆ updateControllerSetpoints()

◆ updateControllerStateMeasurements()

◆ updatePIDConfig()

Member Data Documentation

◆ angular_error_

◆ angular_pid_config_

◆ cartesian_orientation_pids_

◆ cartesian_position_pids_

◆ command_frame_transform_

◆ command_frame_transform_stamp_

◆ joint_model_group_

◆ loop_rate_

◆ move_group_name_

◆ nh_

◆ planning_frame_

◆ planning_scene_monitor_

◆ robot_model_

◆ servo_

◆ stop_requested_

◆ target_pose_

◆ target_pose_mtx_

◆ target_pose_sub_

◆ transform_buffer_

◆ transform_listener_

◆ twist_stamped_pub_

◆ x_pid_config_

◆ y_pid_config_

◆ z_pid_config_