#include <servo_calcs.h>

Public Member Functions
void	changeRobotLinkCommandFrame (const std::string &new_command_frame)
	Change the controlled link. Often, this is the end effector This must be a link on the robot since MoveIt tracks the transform (not tf) More...

bool	getCommandFrameTransform (Eigen::Isometry3d &transform)

bool	getCommandFrameTransform (geometry_msgs::TransformStamped &transform)

bool	getEEFrameTransform (Eigen::Isometry3d &transform)

bool	getEEFrameTransform (geometry_msgs::TransformStamped &transform)

	ServoCalcs (ros::NodeHandle &nh, ServoParameters &parameters, const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor)

void	setPaused (bool paused)
	Pause or unpause processing servo commands while keeping the timers alive. More...

void	start ()
	Start the timer where we do work and publish outputs. More...

	~ServoCalcs ()

Private Member Functions
bool	addJointIncrements (sensor_msgs::JointState &output, const Eigen::VectorXd &increments) const

void	applyVelocityScaling (Eigen::ArrayXd &delta_theta, double singularity_scale)

void	calculateJointVelocities (sensor_msgs::JointState &joint_state, const Eigen::ArrayXd &delta_theta)
	Convert an incremental position command to joint velocity message. More...

void	calculateSingleIteration ()
	Do calculations for a single iteration. Publish one outgoing command. More...

bool	cartesianServoCalcs (geometry_msgs::TwistStamped &cmd, trajectory_msgs::JointTrajectory &joint_trajectory)
	Do servoing calculations for Cartesian twist commands. More...

bool	changeControlDimensions (moveit_msgs::ChangeControlDimensions::Request &req, moveit_msgs::ChangeControlDimensions::Response &res)
	Service callback for changing servoing dimensions (e.g. ignore rotation about X) More...

bool	changeDriftDimensions (moveit_msgs::ChangeDriftDimensions::Request &req, moveit_msgs::ChangeDriftDimensions::Response &res)

void	collisionVelocityScaleCB (const std_msgs::Float64ConstPtr &msg)

void	composeJointTrajMessage (const sensor_msgs::JointState &joint_state, trajectory_msgs::JointTrajectory &joint_trajectory) const
	Compose the outgoing JointTrajectory message. More...

bool	convertDeltasToOutgoingCmd (trajectory_msgs::JointTrajectory &joint_trajectory)
	Convert joint deltas to an outgoing JointTrajectory command. This happens for joint commands and Cartesian commands. More...

bool	enforcePositionLimits (sensor_msgs::JointState &joint_state)
	Avoid overshooting joint limits. More...

void	enforceVelLimits (Eigen::ArrayXd &delta_theta)
	Scale the delta theta to match joint velocity/acceleration limits. More...

void	insertRedundantPointsIntoTrajectory (trajectory_msgs::JointTrajectory &joint_trajectory, int count) const
	Gazebo simulations have very strict message timestamp requirements. Satisfy Gazebo by stuffing multiple messages into one. More...

void	jointCmdCB (const control_msgs::JointJogConstPtr &msg)

bool	jointServoCalcs (const control_msgs::JointJog &cmd, trajectory_msgs::JointTrajectory &joint_trajectory)
	Do servoing calculations for direct commands to a joint. More...

void	jointStateCB (const sensor_msgs::JointStateConstPtr &msg)

void	lowPassFilterPositions (sensor_msgs::JointState &joint_state)
	Smooth position commands with a lowpass filter. More...

void	mainCalcLoop ()
	Run the main calculation loop. More...

void	removeDimension (Eigen::MatrixXd &matrix, Eigen::VectorXd &delta_x, unsigned int row_to_remove)

void	resetLowPassFilters (const sensor_msgs::JointState &joint_state)
	Set the filters to the specified values. More...

bool	resetServoStatus (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
	Service callback to reset Servo status, e.g. so the arm can move again after a collision. More...

Eigen::VectorXd	scaleCartesianCommand (const geometry_msgs::TwistStamped &command) const
	If incoming velocity commands are from a unitless joystick, scale them to physical units. Also, multiply by timestep to calculate a position change. More...

Eigen::VectorXd	scaleJointCommand (const control_msgs::JointJog &command) const
	If incoming velocity commands are from a unitless joystick, scale them to physical units. Also, multiply by timestep to calculate a position change. More...

void	stop ()
	Stop the currently running thread. More...

void	suddenHalt (trajectory_msgs::JointTrajectory &joint_trajectory)
	Suddenly halt for a joint limit or other critical issue. Is handled differently for position vs. velocity control. More...

void	twistStampedCB (const geometry_msgs::TwistStampedConstPtr &msg)

void	updateJoints ()
	Parse the incoming joint msg for the joints of our MoveGroup. More...

double	velocityScalingFactorForSingularity (const Eigen::VectorXd &commanded_velocity, const Eigen::JacobiSVD< Eigen::MatrixXd > &svd, const Eigen::MatrixXd &pseudo_inverse)
	Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion. More...

Private Attributes
double	collision_velocity_scale_ = 1.0

ros::Subscriber	collision_velocity_scale_sub_

std::array< bool, 6 >	control_dimensions_ = { { true, true, true, true, true, true } }

ros::ServiceServer	control_dimensions_server_

moveit::core::RobotStatePtr	current_state_

Eigen::ArrayXd	delta_theta_

std::array< bool, 6 >	drift_dimensions_ = { { false, false, false, false, false, false } }

ros::ServiceServer	drift_dimensions_server_

const int	gazebo_redundant_message_count_ = 30

bool	have_nonzero_command_ = false

bool	have_nonzero_joint_command_ = false

bool	have_nonzero_twist_stamped_ = false

std::condition_variable	input_cv_

std::mutex	input_mutex_

sensor_msgs::JointState	internal_joint_state_

ros::Subscriber	joint_cmd_sub_

bool	joint_command_is_stale_ = false

const moveit::core::JointModelGroup *	joint_model_group_

control_msgs::JointJog	joint_servo_cmd_

std::map< std::string, std::size_t >	joint_state_name_map_

ros::Subscriber	joint_state_sub_

trajectory_msgs::JointTrajectoryConstPtr	last_sent_command_

control_msgs::JointJogConstPtr	latest_joint_cmd_

ros::Time	latest_joint_command_stamp_ = ros::Time(0.)

bool	latest_nonzero_joint_cmd_ = false

bool	latest_nonzero_twist_stamped_ = false

ros::Time	latest_twist_command_stamp_ = ros::Time(0.)

geometry_msgs::TwistStampedConstPtr	latest_twist_stamped_

bool	new_input_cmd_ = false

ros::NodeHandle	nh_

uint	num_joints_

bool	ok_to_publish_ = false

sensor_msgs::JointState	original_joint_state_

ros::Publisher	outgoing_cmd_pub_

ServoParameters &	parameters_

std::atomic< bool >	paused_

planning_scene_monitor::PlanningSceneMonitorPtr	planning_scene_monitor_

std::vector< LowPassFilter >	position_filters_

Eigen::ArrayXd	prev_joint_velocity_

ros::ServiceServer	reset_servo_status_

StatusCode	status_ = StatusCode::NO_WARNING

ros::Publisher	status_pub_

bool	stop_requested_

Eigen::Isometry3d	tf_moveit_to_ee_frame_

Eigen::Isometry3d	tf_moveit_to_robot_cmd_frame_

std::thread	thread_

bool	twist_command_is_stale_ = false

geometry_msgs::TwistStamped	twist_stamped_cmd_

ros::Subscriber	twist_stamped_sub_

bool	updated_filters_ = false

bool	wait_for_servo_commands_ = true

ros::Publisher	worst_case_stop_time_pub_

int	zero_velocity_count_ = 0

Friends
class	ServoFixture

Detailed Description

Definition at line 105 of file servo_calcs.h.

Constructor & Destructor Documentation

◆ ServoCalcs()

moveit_servo::ServoCalcs::ServoCalcs	(	ros::NodeHandle &	nh,
		ServoParameters &	parameters,
		const planning_scene_monitor::PlanningSceneMonitorPtr &	planning_scene_monitor
	)

Definition at line 87 of file servo_calcs.cpp.

◆ ~ServoCalcs()

moveit_servo::ServoCalcs::~ServoCalcs ( )

Definition at line 157 of file servo_calcs.cpp.

Member Function Documentation

◆ addJointIncrements()

bool moveit_servo::ServoCalcs::addJointIncrements	(	sensor_msgs::JointState &	output,
		const Eigen::VectorXd &	increments
	)		const

private

Definition at line 984 of file servo_calcs.cpp.

◆ applyVelocityScaling()

void moveit_servo::ServoCalcs::applyVelocityScaling	(	Eigen::ArrayXd &	delta_theta,
		double	singularity_scale
	)

private

Slow motion down if close to singularity or collision.

Parameters

delta_theta	motion command, used in calculating new_joint_tray
singularity_scale	tells how close we are to a singularity

Definition at line 670 of file servo_calcs.cpp.

◆ calculateJointVelocities()

void moveit_servo::ServoCalcs::calculateJointVelocities	(	sensor_msgs::JointState &	joint_state,
		const Eigen::ArrayXd &	delta_theta
	)

private

Convert an incremental position command to joint velocity message.

Definition at line 635 of file servo_calcs.cpp.

◆ calculateSingleIteration()

void moveit_servo::ServoCalcs::calculateSingleIteration ( )

private

Do calculations for a single iteration. Publish one outgoing command.

Definition at line 268 of file servo_calcs.cpp.

◆ cartesianServoCalcs()

bool moveit_servo::ServoCalcs::cartesianServoCalcs	(	geometry_msgs::TwistStamped &	cmd,
		trajectory_msgs::JointTrajectory &	joint_trajectory
	)

private

Do servoing calculations for Cartesian twist commands.

Definition at line 434 of file servo_calcs.cpp.

◆ changeControlDimensions()

bool moveit_servo::ServoCalcs::changeControlDimensions	(	moveit_msgs::ChangeControlDimensions::Request &	req,
		moveit_msgs::ChangeControlDimensions::Response &	res
	)

private

Service callback for changing servoing dimensions (e.g. ignore rotation about X)

Definition at line 1112 of file servo_calcs.cpp.

◆ changeDriftDimensions()

bool moveit_servo::ServoCalcs::changeDriftDimensions	(	moveit_msgs::ChangeDriftDimensions::Request &	req,
		moveit_msgs::ChangeDriftDimensions::Response &	res
	)

private

Allow drift in certain dimensions. For example, may allow the wrist to rotate freely. This can help avoid singularities.

Parameters

request	the service request
response	the service response

Returns: true if the adjustment was made

Definition at line 1098 of file servo_calcs.cpp.

◆ changeRobotLinkCommandFrame()

void moveit_servo::ServoCalcs::changeRobotLinkCommandFrame ( const std::string & new_command_frame )

Change the controlled link. Often, this is the end effector This must be a link on the robot since MoveIt tracks the transform (not tf)

Definition at line 1137 of file servo_calcs.cpp.

◆ collisionVelocityScaleCB()

void moveit_servo::ServoCalcs::collisionVelocityScaleCB ( const std_msgs::Float64ConstPtr & msg )

private

Definition at line 1093 of file servo_calcs.cpp.

◆ composeJointTrajMessage()

void moveit_servo::ServoCalcs::composeJointTrajMessage	(	const sensor_msgs::JointState &	joint_state,
		trajectory_msgs::JointTrajectory &	joint_trajectory
	)		const

private

Compose the outgoing JointTrajectory message.

Definition at line 643 of file servo_calcs.cpp.

◆ convertDeltasToOutgoingCmd()

bool moveit_servo::ServoCalcs::convertDeltasToOutgoingCmd ( trajectory_msgs::JointTrajectory & joint_trajectory )

private

Convert joint deltas to an outgoing JointTrajectory command. This happens for joint commands and Cartesian commands.

Definition at line 572 of file servo_calcs.cpp.

◆ enforcePositionLimits()

bool moveit_servo::ServoCalcs::enforcePositionLimits ( sensor_msgs::JointState & joint_state )

private

Avoid overshooting joint limits.

Definition at line 783 of file servo_calcs.cpp.

◆ enforceVelLimits()

void moveit_servo::ServoCalcs::enforceVelLimits ( Eigen::ArrayXd & delta_theta )

private

Scale the delta theta to match joint velocity/acceleration limits.

Definition at line 759 of file servo_calcs.cpp.

◆ getCommandFrameTransform() [1/2]

bool moveit_servo::ServoCalcs::getCommandFrameTransform ( Eigen::Isometry3d & transform )

Get the MoveIt planning link transform. The transform from the MoveIt planning frame to robot_link_command_frame

Parameters

transform the transform that will be calculated

Returns: true if a valid transform was available

Definition at line 1020 of file servo_calcs.cpp.

◆ getCommandFrameTransform() [2/2]

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

Definition at line 1029 of file servo_calcs.cpp.

◆ getEEFrameTransform() [1/2]

bool moveit_servo::ServoCalcs::getEEFrameTransform ( Eigen::Isometry3d & 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 1043 of file servo_calcs.cpp.

◆ getEEFrameTransform() [2/2]

bool moveit_servo::ServoCalcs::getEEFrameTransform ( geometry_msgs::TransformStamped & transform )

Definition at line 1052 of file servo_calcs.cpp.

◆ insertRedundantPointsIntoTrajectory()

void moveit_servo::ServoCalcs::insertRedundantPointsIntoTrajectory	(	trajectory_msgs::JointTrajectory &	joint_trajectory,
		int	count
	)		const

private

Gazebo simulations have very strict message timestamp requirements. Satisfy Gazebo by stuffing multiple messages into one.

Definition at line 602 of file servo_calcs.cpp.

◆ jointCmdCB()

void moveit_servo::ServoCalcs::jointCmdCB ( const control_msgs::JointJogConstPtr & msg )

private

Definition at line 1079 of file servo_calcs.cpp.

◆ jointServoCalcs()

bool moveit_servo::ServoCalcs::jointServoCalcs	(	const control_msgs::JointJog &	cmd,
		trajectory_msgs::JointTrajectory &	joint_trajectory
	)

private

Do servoing calculations for direct commands to a joint.

Definition at line 546 of file servo_calcs.cpp.

◆ jointStateCB()

void moveit_servo::ServoCalcs::jointStateCB ( const sensor_msgs::JointStateConstPtr & msg )

private

◆ lowPassFilterPositions()

void moveit_servo::ServoCalcs::lowPassFilterPositions ( sensor_msgs::JointState & joint_state )

private

Smooth position commands with a lowpass filter.

Definition at line 615 of file servo_calcs.cpp.

◆ mainCalcLoop()

void moveit_servo::ServoCalcs::mainCalcLoop ( )

private

Run the main calculation loop.

Definition at line 228 of file servo_calcs.cpp.

◆ removeDimension()

void moveit_servo::ServoCalcs::removeDimension	(	Eigen::MatrixXd &	matrix,
		Eigen::VectorXd &	delta_x,
		unsigned int	row_to_remove
	)

private

Remove the Jacobian row and the delta-x element of one Cartesian dimension, to take advantage of task redundancy

Parameters

matrix	The Jacobian matrix.
delta_x	Vector of Cartesian delta commands, should be the same size as matrix.rows()
row_to_remove	Dimension that will be allowed to drift, e.g. row_to_remove = 2 allows z-translation drift.

Definition at line 1004 of file servo_calcs.cpp.

◆ resetLowPassFilters()

void moveit_servo::ServoCalcs::resetLowPassFilters ( const sensor_msgs::JointState & joint_state )

private

Set the filters to the specified values.

Definition at line 625 of file servo_calcs.cpp.

◆ resetServoStatus()

bool moveit_servo::ServoCalcs::resetServoStatus	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)

private

Service callback to reset Servo status, e.g. so the arm can move again after a collision.

Definition at line 1126 of file servo_calcs.cpp.

◆ scaleCartesianCommand()

Eigen::VectorXd moveit_servo::ServoCalcs::scaleCartesianCommand ( const geometry_msgs::TwistStamped & command ) const

private

If incoming velocity commands are from a unitless joystick, scale them to physical units. Also, multiply by timestep to calculate a position change.

Definition at line 923 of file servo_calcs.cpp.

◆ scaleJointCommand()

Eigen::VectorXd moveit_servo::ServoCalcs::scaleJointCommand ( const control_msgs::JointJog & command ) const

private

If incoming velocity commands are from a unitless joystick, scale them to physical units. Also, multiply by timestep to calculate a position change.

Definition at line 953 of file servo_calcs.cpp.

◆ setPaused()

void moveit_servo::ServoCalcs::setPaused ( bool paused )

Pause or unpause processing servo commands while keeping the timers alive.

Definition at line 1132 of file servo_calcs.cpp.

◆ start()

void moveit_servo::ServoCalcs::start ( )

Start the timer where we do work and publish outputs.

Definition at line 162 of file servo_calcs.cpp.

◆ stop()

void moveit_servo::ServoCalcs::stop ( )

private

Stop the currently running thread.

Definition at line 207 of file servo_calcs.cpp.

◆ suddenHalt()

void moveit_servo::ServoCalcs::suddenHalt ( trajectory_msgs::JointTrajectory & joint_trajectory )

private

Suddenly halt for a joint limit or other critical issue. Is handled differently for position vs. velocity control.

Definition at line 829 of file servo_calcs.cpp.

◆ twistStampedCB()

void moveit_servo::ServoCalcs::twistStampedCB ( const geometry_msgs::TwistStampedConstPtr & msg )

private

Definition at line 1065 of file servo_calcs.cpp.

◆ updateJoints()

void moveit_servo::ServoCalcs::updateJoints ( )

private

Parse the incoming joint msg for the joints of our MoveGroup.

Definition at line 864 of file servo_calcs.cpp.

◆ velocityScalingFactorForSingularity()

double moveit_servo::ServoCalcs::velocityScalingFactorForSingularity	(	const Eigen::VectorXd &	commanded_velocity,
		const Eigen::JacobiSVD< Eigen::MatrixXd > &	svd,
		const Eigen::MatrixXd &	pseudo_inverse
	)

private

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

Definition at line 694 of file servo_calcs.cpp.

Friends And Related Function Documentation

◆ ServoFixture

friend class ServoFixture

friend

Definition at line 181 of file servo_calcs.h.

Member Data Documentation

◆ collision_velocity_scale_

double moveit_servo::ServoCalcs::collision_velocity_scale_ = 1.0

private

Definition at line 362 of file servo_calcs.h.

◆ collision_velocity_scale_sub_

ros::Subscriber moveit_servo::ServoCalcs::collision_velocity_scale_sub_

private

Definition at line 344 of file servo_calcs.h.

◆ control_dimensions_

std::array<bool, 6> moveit_servo::ServoCalcs::control_dimensions_ = { { true, true, true, true, true, true } }

private

Definition at line 376 of file servo_calcs.h.

◆ control_dimensions_server_

ros::ServiceServer moveit_servo::ServoCalcs::control_dimensions_server_

private

Definition at line 349 of file servo_calcs.h.

◆ current_state_

moveit::core::RobotStatePtr moveit_servo::ServoCalcs::current_state_

private

Definition at line 326 of file servo_calcs.h.

◆ delta_theta_

Eigen::ArrayXd moveit_servo::ServoCalcs::delta_theta_

private

Definition at line 365 of file servo_calcs.h.

◆ drift_dimensions_

std::array<bool, 6> moveit_servo::ServoCalcs::drift_dimensions_ = { { false, false, false, false, false, false } }

private

Definition at line 373 of file servo_calcs.h.

◆ drift_dimensions_server_

ros::ServiceServer moveit_servo::ServoCalcs::drift_dimensions_server_

private

Definition at line 348 of file servo_calcs.h.

◆ gazebo_redundant_message_count_

const int moveit_servo::ServoCalcs::gazebo_redundant_message_count_ = 30

private

Definition at line 368 of file servo_calcs.h.

◆ have_nonzero_command_

bool moveit_servo::ServoCalcs::have_nonzero_command_ = false

private

Definition at line 318 of file servo_calcs.h.

◆ have_nonzero_joint_command_

bool moveit_servo::ServoCalcs::have_nonzero_joint_command_ = false

private

Definition at line 317 of file servo_calcs.h.

◆ have_nonzero_twist_stamped_

bool moveit_servo::ServoCalcs::have_nonzero_twist_stamped_ = false

private

Definition at line 316 of file servo_calcs.h.

◆ input_cv_

std::condition_variable moveit_servo::ServoCalcs::input_cv_

private

Definition at line 390 of file servo_calcs.h.

◆ input_mutex_

std::mutex moveit_servo::ServoCalcs::input_mutex_

mutableprivate

Definition at line 379 of file servo_calcs.h.

◆ internal_joint_state_

sensor_msgs::JointState moveit_servo::ServoCalcs::internal_joint_state_

private

Definition at line 333 of file servo_calcs.h.

◆ joint_cmd_sub_

ros::Subscriber moveit_servo::ServoCalcs::joint_cmd_sub_

private

Definition at line 343 of file servo_calcs.h.

◆ joint_command_is_stale_

bool moveit_servo::ServoCalcs::joint_command_is_stale_ = false

private

Definition at line 360 of file servo_calcs.h.

◆ joint_model_group_

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

private

Definition at line 324 of file servo_calcs.h.

◆ joint_servo_cmd_

control_msgs::JointJog moveit_servo::ServoCalcs::joint_servo_cmd_

private

Definition at line 322 of file servo_calcs.h.

◆ joint_state_name_map_

std::map<std::string, std::size_t> moveit_servo::ServoCalcs::joint_state_name_map_

private

Definition at line 334 of file servo_calcs.h.

◆ joint_state_sub_

ros::Subscriber moveit_servo::ServoCalcs::joint_state_sub_

private

Definition at line 341 of file servo_calcs.h.

◆ last_sent_command_

trajectory_msgs::JointTrajectoryConstPtr moveit_servo::ServoCalcs::last_sent_command_

private

Definition at line 338 of file servo_calcs.h.

◆ latest_joint_cmd_

control_msgs::JointJogConstPtr moveit_servo::ServoCalcs::latest_joint_cmd_

private

Definition at line 383 of file servo_calcs.h.

◆ latest_joint_command_stamp_

ros::Time moveit_servo::ServoCalcs::latest_joint_command_stamp_ = ros::Time(0.)

private

Definition at line 385 of file servo_calcs.h.

◆ latest_nonzero_joint_cmd_

bool moveit_servo::ServoCalcs::latest_nonzero_joint_cmd_ = false

private

Definition at line 387 of file servo_calcs.h.

◆ latest_nonzero_twist_stamped_

bool moveit_servo::ServoCalcs::latest_nonzero_twist_stamped_ = false

private

Definition at line 386 of file servo_calcs.h.

◆ latest_twist_command_stamp_

ros::Time moveit_servo::ServoCalcs::latest_twist_command_stamp_ = ros::Time(0.)

private

Definition at line 384 of file servo_calcs.h.

◆ latest_twist_stamped_

geometry_msgs::TwistStampedConstPtr moveit_servo::ServoCalcs::latest_twist_stamped_

private

Definition at line 382 of file servo_calcs.h.

◆ new_input_cmd_

bool moveit_servo::ServoCalcs::new_input_cmd_ = false

private

Definition at line 391 of file servo_calcs.h.

◆ nh_

ros::NodeHandle moveit_servo::ServoCalcs::nh_

private

Definition at line 297 of file servo_calcs.h.

◆ num_joints_

uint moveit_servo::ServoCalcs::num_joints_

private

Definition at line 370 of file servo_calcs.h.

◆ ok_to_publish_

bool moveit_servo::ServoCalcs::ok_to_publish_ = false

private

Definition at line 361 of file servo_calcs.h.

◆ original_joint_state_

sensor_msgs::JointState moveit_servo::ServoCalcs::original_joint_state_

private

Definition at line 333 of file servo_calcs.h.

◆ outgoing_cmd_pub_

ros::Publisher moveit_servo::ServoCalcs::outgoing_cmd_pub_

private

Definition at line 347 of file servo_calcs.h.

◆ parameters_

ServoParameters& moveit_servo::ServoCalcs::parameters_

private

Definition at line 300 of file servo_calcs.h.

◆ paused_

std::atomic<bool> moveit_servo::ServoCalcs::paused_

private

Definition at line 358 of file servo_calcs.h.

◆ planning_scene_monitor_

planning_scene_monitor::PlanningSceneMonitorPtr moveit_servo::ServoCalcs::planning_scene_monitor_

private

Definition at line 303 of file servo_calcs.h.

◆ position_filters_

std::vector<LowPassFilter> moveit_servo::ServoCalcs::position_filters_

private

Definition at line 336 of file servo_calcs.h.

◆ prev_joint_velocity_

Eigen::ArrayXd moveit_servo::ServoCalcs::prev_joint_velocity_

private

Definition at line 366 of file servo_calcs.h.

◆ reset_servo_status_

ros::ServiceServer moveit_servo::ServoCalcs::reset_servo_status_

private

Definition at line 350 of file servo_calcs.h.

◆ status_

StatusCode moveit_servo::ServoCalcs::status_ = StatusCode::NO_WARNING

private

Definition at line 357 of file servo_calcs.h.

◆ status_pub_

ros::Publisher moveit_servo::ServoCalcs::status_pub_

private

Definition at line 345 of file servo_calcs.h.

◆ stop_requested_

bool moveit_servo::ServoCalcs::stop_requested_

private

Definition at line 354 of file servo_calcs.h.

◆ tf_moveit_to_ee_frame_

Eigen::Isometry3d moveit_servo::ServoCalcs::tf_moveit_to_ee_frame_

private

Definition at line 381 of file servo_calcs.h.

◆ tf_moveit_to_robot_cmd_frame_

Eigen::Isometry3d moveit_servo::ServoCalcs::tf_moveit_to_robot_cmd_frame_

private

Definition at line 380 of file servo_calcs.h.

◆ thread_

std::thread moveit_servo::ServoCalcs::thread_

private

Definition at line 353 of file servo_calcs.h.

◆ twist_command_is_stale_

bool moveit_servo::ServoCalcs::twist_command_is_stale_ = false

private

Definition at line 359 of file servo_calcs.h.

◆ twist_stamped_cmd_

geometry_msgs::TwistStamped moveit_servo::ServoCalcs::twist_stamped_cmd_

private

Definition at line 321 of file servo_calcs.h.

◆ twist_stamped_sub_

ros::Subscriber moveit_servo::ServoCalcs::twist_stamped_sub_

private

Definition at line 342 of file servo_calcs.h.

◆ updated_filters_

bool moveit_servo::ServoCalcs::updated_filters_ = false

private

Definition at line 313 of file servo_calcs.h.

◆ wait_for_servo_commands_

bool moveit_servo::ServoCalcs::wait_for_servo_commands_ = true

private

Definition at line 310 of file servo_calcs.h.

◆ worst_case_stop_time_pub_

ros::Publisher moveit_servo::ServoCalcs::worst_case_stop_time_pub_

private

Definition at line 346 of file servo_calcs.h.

◆ zero_velocity_count_

int moveit_servo::ServoCalcs::zero_velocity_count_ = 0

private

Definition at line 307 of file servo_calcs.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

◆ ServoCalcs()

◆ ~ServoCalcs()

Member Function Documentation

◆ addJointIncrements()

◆ applyVelocityScaling()

◆ calculateJointVelocities()

◆ calculateSingleIteration()

◆ cartesianServoCalcs()

◆ changeControlDimensions()

◆ changeDriftDimensions()

◆ changeRobotLinkCommandFrame()

◆ collisionVelocityScaleCB()

◆ composeJointTrajMessage()

◆ convertDeltasToOutgoingCmd()

◆ enforcePositionLimits()

◆ enforceVelLimits()

◆ getCommandFrameTransform() [1/2]

◆ getCommandFrameTransform() [2/2]

◆ getEEFrameTransform() [1/2]

◆ getEEFrameTransform() [2/2]

◆ insertRedundantPointsIntoTrajectory()

◆ jointCmdCB()

◆ jointServoCalcs()

◆ jointStateCB()

◆ lowPassFilterPositions()

◆ mainCalcLoop()

◆ removeDimension()

◆ resetLowPassFilters()

◆ resetServoStatus()

◆ scaleCartesianCommand()

◆ scaleJointCommand()

◆ setPaused()

◆ start()

◆ stop()

◆ suddenHalt()

◆ twistStampedCB()

◆ updateJoints()

◆ velocityScalingFactorForSingularity()

Friends And Related Function Documentation

◆ ServoFixture

Member Data Documentation

◆ collision_velocity_scale_

◆ collision_velocity_scale_sub_

◆ control_dimensions_

◆ control_dimensions_server_

◆ current_state_

◆ delta_theta_

◆ drift_dimensions_

◆ drift_dimensions_server_

◆ gazebo_redundant_message_count_

◆ have_nonzero_command_

◆ have_nonzero_joint_command_

◆ have_nonzero_twist_stamped_

◆ input_cv_

◆ input_mutex_

◆ internal_joint_state_

◆ joint_cmd_sub_

◆ joint_command_is_stale_

◆ joint_model_group_

◆ joint_servo_cmd_

◆ joint_state_name_map_

◆ joint_state_sub_

◆ last_sent_command_

◆ latest_joint_cmd_

◆ latest_joint_command_stamp_

◆ latest_nonzero_joint_cmd_

◆ latest_nonzero_twist_stamped_

◆ latest_twist_command_stamp_

◆ latest_twist_stamped_

◆ new_input_cmd_

◆ nh_

◆ num_joints_

◆ ok_to_publish_

◆ original_joint_state_