#include <servo_calcs.h>

Collaboration diagram for moveit_servo::ServoCalcs:

[legend]

Public Member Functions
	ServoCalcs (const rclcpp::Node::SharedPtr &node, const std::shared_ptr< const moveit_servo::ServoParameters > &parameters, const planning_scene_monitor::PlanningSceneMonitorPtr &planning_scene_monitor)

	~ServoCalcs ()

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

bool	getCommandFrameTransform (Eigen::Isometry3d &transform)

bool	getCommandFrameTransform (geometry_msgs::msg::TransformStamped &transform)

bool	getEEFrameTransform (Eigen::Isometry3d &transform)

bool	getEEFrameTransform (geometry_msgs::msg::TransformStamped &transform)

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

Protected Member Functions
void	mainCalcLoop ()
	Run the main calculation loop. More...

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

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

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

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

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

bool	checkValidCommand (const control_msgs::msg::JointJog &cmd)

bool	checkValidCommand (const geometry_msgs::msg::TwistStamped &cmd)

Eigen::VectorXd	scaleCartesianCommand (const geometry_msgs::msg::TwistStamped &command)
	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::msg::JointJog &command)
	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	filteredHalt (trajectory_msgs::msg::JointTrajectory &joint_trajectory)
	Come to a halt in a smooth way. Apply a smoothing plugin, if one is configured. More...

void	suddenHalt (sensor_msgs::msg::JointState &joint_state, const std::vector< const moveit::core::JointModel * > &joints_to_halt) const
	Suddenly halt for a joint limit or other critical issue. Is handled differently for position vs. velocity control. More...

std::vector< const moveit::core::JointModel * >	enforcePositionLimits (sensor_msgs::msg::JointState &joint_state) const
	Avoid overshooting joint limits. More...

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

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

bool	internalServoUpdate (Eigen::ArrayXd &delta_theta, trajectory_msgs::msg::JointTrajectory &joint_trajectory, const ServoType servo_type)
	Handles all aspects of the servoing after the desired joint commands are established Joint and Cartesian calcs feed into here Handles limit enforcement, internal state updated, collision scaling, and publishing the commands. More...

bool	applyJointUpdate (const Eigen::ArrayXd &delta_theta, sensor_msgs::msg::JointState &joint_state)
	Joint-wise update of a sensor_msgs::msg::JointState with given delta's Also filters and calculates the previous velocity. More...

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

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

void	removeDriftDimensions (Eigen::MatrixXd &matrix, Eigen::VectorXd &delta_x)

void	enforceControlDimensions (geometry_msgs::msg::TwistStamped &command)

void	jointStateCB (const sensor_msgs::msg::JointState::SharedPtr msg)

void	twistStampedCB (const geometry_msgs::msg::TwistStamped::ConstSharedPtr &msg)

void	jointCmdCB (const control_msgs::msg::JointJog::ConstSharedPtr &msg)

void	collisionVelocityScaleCB (const std_msgs::msg::Float64::ConstSharedPtr &msg)

void	changeDriftDimensions (const std::shared_ptr< moveit_msgs::srv::ChangeDriftDimensions::Request > &req, const std::shared_ptr< moveit_msgs::srv::ChangeDriftDimensions::Response > &res)

void	changeControlDimensions (const std::shared_ptr< moveit_msgs::srv::ChangeControlDimensions::Request > &req, const std::shared_ptr< moveit_msgs::srv::ChangeControlDimensions::Response > &res)
	Start the main calculation timer. More...

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

rcl_interfaces::msg::SetParametersResult	robotLinkCommandFrameCallback (const rclcpp::Parameter &parameter)

Protected Attributes
std::shared_ptr< rclcpp::Node >	node_

const std::shared_ptr< const moveit_servo::ServoParameters >	parameters_

planning_scene_monitor::PlanningSceneMonitorPtr	planning_scene_monitor_

int	zero_velocity_count_ = 0

bool	wait_for_servo_commands_ = true

bool	updated_filters_ = false

bool	have_nonzero_twist_stamped_ = false

bool	have_nonzero_joint_command_ = false

bool	have_nonzero_command_ = false

geometry_msgs::msg::TwistStamped	twist_stamped_cmd_

control_msgs::msg::JointJog	joint_servo_cmd_

const moveit::core::JointModelGroup *	joint_model_group_

moveit::core::RobotStatePtr	current_state_

sensor_msgs::msg::JointState	internal_joint_state_

sensor_msgs::msg::JointState	original_joint_state_

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

std::shared_ptr< online_signal_smoothing::SmoothingBaseClass >	smoother_

trajectory_msgs::msg::JointTrajectory::SharedPtr	last_sent_command_

rclcpp::Subscription< sensor_msgs::msg::JointState >::SharedPtr	joint_state_sub_

rclcpp::Subscription< geometry_msgs::msg::TwistStamped >::SharedPtr	twist_stamped_sub_

rclcpp::Subscription< control_msgs::msg::JointJog >::SharedPtr	joint_cmd_sub_

rclcpp::Subscription< std_msgs::msg::Float64 >::SharedPtr	collision_velocity_scale_sub_

rclcpp::Publisher< std_msgs::msg::Int8 >::SharedPtr	status_pub_

rclcpp::Publisher< trajectory_msgs::msg::JointTrajectory >::SharedPtr	trajectory_outgoing_cmd_pub_

rclcpp::Publisher< std_msgs::msg::Float64MultiArray >::SharedPtr	multiarray_outgoing_cmd_pub_

rclcpp::Service< moveit_msgs::srv::ChangeControlDimensions >::SharedPtr	control_dimensions_server_

rclcpp::Service< moveit_msgs::srv::ChangeDriftDimensions >::SharedPtr	drift_dimensions_server_

rclcpp::Service< std_srvs::srv::Empty >::SharedPtr	reset_servo_status_

std::thread	thread_

bool	stop_requested_

std::atomic< bool >	done_stopping_

StatusCode	status_ = StatusCode::NO_WARNING

std::atomic< bool >	paused_

bool	twist_command_is_stale_ = false

bool	joint_command_is_stale_ = false

bool	ok_to_publish_ = false

double	collision_velocity_scale_ = 1.0

Eigen::ArrayXd	delta_theta_

const int	gazebo_redundant_message_count_ = 30

unsigned int	num_joints_

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

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

std::mutex	main_loop_mutex_

Eigen::Isometry3d	tf_moveit_to_robot_cmd_frame_

Eigen::Isometry3d	tf_moveit_to_ee_frame_

geometry_msgs::msg::TwistStamped::ConstSharedPtr	latest_twist_stamped_

control_msgs::msg::JointJog::ConstSharedPtr	latest_joint_cmd_

rclcpp::Time	latest_twist_command_stamp_ = rclcpp::Time(0., RCL_ROS_TIME)

rclcpp::Time	latest_joint_command_stamp_ = rclcpp::Time(0., RCL_ROS_TIME)

bool	latest_twist_cmd_is_nonzero_ = false

bool	latest_joint_cmd_is_nonzero_ = false

std::condition_variable	input_cv_

bool	new_input_cmd_ = false

std::string	robot_link_command_frame_

pluginlib::ClassLoader< online_signal_smoothing::SmoothingBaseClass >	smoothing_loader_

kinematics::KinematicsBaseConstPtr	ik_solver_

Eigen::Isometry3d	ik_base_to_tip_frame_

bool	use_inv_jacobian_ = false

Detailed Description

Definition at line 79 of file servo_calcs.h.

Constructor & Destructor Documentation

◆ ServoCalcs()

moveit_servo::ServoCalcs::ServoCalcs	(	const rclcpp::Node::SharedPtr &	node,
		const std::shared_ptr< const moveit_servo::ServoParameters > &	parameters,
		const planning_scene_monitor::PlanningSceneMonitorPtr &	planning_scene_monitor
	)

Definition at line 65 of file servo_calcs.cpp.

◆ ~ServoCalcs()

moveit_servo::ServoCalcs::~ServoCalcs ( )

Definition at line 205 of file servo_calcs.cpp.

Here is the call graph for this function:

Member Function Documentation

◆ applyJointUpdate()

bool moveit_servo::ServoCalcs::applyJointUpdate	(	const Eigen::ArrayXd &	delta_theta,
		sensor_msgs::msg::JointState &	joint_state
	)

protected

Joint-wise update of a sensor_msgs::msg::JointState with given delta's Also filters and calculates the previous velocity.

Parameters

delta_theta	Eigen vector of joint delta's
joint_state	The joint state msg being updated
previous_vel	Eigen vector of previous velocities being updated

Returns: Returns false if there is a problem, true otherwise

Definition at line 737 of file servo_calcs.cpp.

◆ calculateSingleIteration()

void moveit_servo::ServoCalcs::calculateSingleIteration ( )

protected

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

Definition at line 318 of file servo_calcs.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ cartesianServoCalcs()

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

protected

Do servoing calculations for Cartesian twist commands.

Definition at line 520 of file servo_calcs.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ changeControlDimensions()

void moveit_servo::ServoCalcs::changeControlDimensions	(	const std::shared_ptr< moveit_msgs::srv::ChangeControlDimensions::Request > &	req,
		const std::shared_ptr< moveit_msgs::srv::ChangeControlDimensions::Response > &	res
	)

protected

Start the main calculation timer.

Definition at line 1200 of file servo_calcs.cpp.

◆ changeDriftDimensions()

void moveit_servo::ServoCalcs::changeDriftDimensions	(	const std::shared_ptr< moveit_msgs::srv::ChangeDriftDimensions::Request > &	req,
		const std::shared_ptr< moveit_msgs::srv::ChangeDriftDimensions::Response > &	res
	)

protected

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 1187 of file servo_calcs.cpp.

◆ checkValidCommand() [1/2]

bool moveit_servo::ServoCalcs::checkValidCommand ( const control_msgs::msg::JointJog & cmd )

protected

Checks a JointJog msg for valid (non-NaN) velocities

Parameters

cmd	the desired joint servo command

Returns: true if this represents a valid joint servo command, false otherwise

Definition at line 948 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ checkValidCommand() [2/2]

bool moveit_servo::ServoCalcs::checkValidCommand ( const geometry_msgs::msg::TwistStamped & cmd )

protected

Checks a TwistStamped msg for valid (non-NaN) velocities

Parameters

cmd	the desired twist servo command

Returns: true if this represents a valid servo twist command, false otherwise

Definition at line 963 of file servo_calcs.cpp.

◆ collisionVelocityScaleCB()

void moveit_servo::ServoCalcs::collisionVelocityScaleCB ( const std_msgs::msg::Float64::ConstSharedPtr & msg )

protected

Definition at line 1182 of file servo_calcs.cpp.

◆ composeJointTrajMessage()

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

protected

Compose the outgoing JointTrajectory message.

Definition at line 791 of file servo_calcs.cpp.

◆ enforceControlDimensions()

void moveit_servo::ServoCalcs::enforceControlDimensions ( geometry_msgs::msg::TwistStamped & command )

protected

Uses control_dimensions_ to set the incoming twist command values to 0 in uncontrolled directions

Parameters

command TwistStamped msg being used in the Cartesian calcs process

Definition at line 1091 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ enforcePositionLimits()

std::vector< const moveit::core::JointModel * > moveit_servo::ServoCalcs::enforcePositionLimits ( sensor_msgs::msg::JointState & joint_state ) const

protected

Avoid overshooting joint limits.

Returns: Vector of the joints that would move farther past position margin limits

Definition at line 818 of file servo_calcs.cpp.

Here is the call graph for this function:

◆ filteredHalt()

void moveit_servo::ServoCalcs::filteredHalt ( trajectory_msgs::msg::JointTrajectory & joint_trajectory )

protected

Come to a halt in a smooth way. Apply a smoothing plugin, if one is configured.

Definition at line 870 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ 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 1108 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ getCommandFrameTransform() [2/2]

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

Definition at line 1117 of file servo_calcs.cpp.

Here is the call graph for this function:

◆ 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 1131 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ getEEFrameTransform() [2/2]

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

Definition at line 1140 of file servo_calcs.cpp.

Here is the call graph for this function:

◆ insertRedundantPointsIntoTrajectory()

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

protected

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

Definition at line 770 of file servo_calcs.cpp.

◆ internalServoUpdate()

bool moveit_servo::ServoCalcs::internalServoUpdate	(	Eigen::ArrayXd &	delta_theta,
		trajectory_msgs::msg::JointTrajectory &	joint_trajectory,
		const ServoType	servo_type
	)

protected

Handles all aspects of the servoing after the desired joint commands are established Joint and Cartesian calcs feed into here Handles limit enforcement, internal state updated, collision scaling, and publishing the commands.

Parameters

delta_theta	Eigen vector of joint delta's, from joint or Cartesian servo calcs
joint_trajectory	Output trajectory message

Definition at line 668 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ jointCmdCB()

void moveit_servo::ServoCalcs::jointCmdCB ( const control_msgs::msg::JointJog::ConstSharedPtr & msg )

protected

Definition at line 1168 of file servo_calcs.cpp.

Here is the call graph for this function:

◆ jointServoCalcs()

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

protected

Do servoing calculations for direct commands to a joint.

Definition at line 654 of file servo_calcs.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ jointStateCB()

void moveit_servo::ServoCalcs::jointStateCB ( const sensor_msgs::msg::JointState::SharedPtr msg )

protected

◆ mainCalcLoop()

void moveit_servo::ServoCalcs::mainCalcLoop ( )

protected

Run the main calculation loop.

Definition at line 277 of file servo_calcs.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ removeDimension()

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

protected

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 1060 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ removeDriftDimensions()

void moveit_servo::ServoCalcs::removeDriftDimensions	(	Eigen::MatrixXd &	matrix,
		Eigen::VectorXd &	delta_x
	)

protected

Removes all of the drift dimensions from the jacobian and delta-x element

Parameters

matrix	The Jacobian matrix.
delta_x	Vector of Cartesian delta commands, should be the same size as matrix.rows()

Definition at line 1076 of file servo_calcs.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ resetLowPassFilters()

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

protected

Set the filters to the specified values.

Definition at line 785 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ resetServoStatus()

bool moveit_servo::ServoCalcs::resetServoStatus	(	const std::shared_ptr< std_srvs::srv::Empty::Request > &	req,
		const std::shared_ptr< std_srvs::srv::Empty::Response > &	res
	)

protected

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

Definition at line 1213 of file servo_calcs.cpp.

◆ robotLinkCommandFrameCallback()

rcl_interfaces::msg::SetParametersResult moveit_servo::ServoCalcs::robotLinkCommandFrameCallback ( const rclcpp::Parameter & parameter )

protected

Definition at line 509 of file servo_calcs.cpp.

◆ scaleCartesianCommand()

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

protected

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

Returns: a vector of position deltas

Definition at line 991 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ scaleJointCommand()

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

protected

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

Returns: a vector of position deltas

Definition at line 1025 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ setPaused()

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

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

Definition at line 1220 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ start()

void moveit_servo::ServoCalcs::start ( )

Start the timer where we do work and publish outputs.

Definition at line 210 of file servo_calcs.cpp.

Here is the call graph for this function:

◆ stop()

void moveit_servo::ServoCalcs::stop ( )

protected

Stop the currently running thread.

Definition at line 256 of file servo_calcs.cpp.

Here is the caller graph for this function:

◆ suddenHalt()

void moveit_servo::ServoCalcs::suddenHalt	(	sensor_msgs::msg::JointState &	joint_state,
		const std::vector< const moveit::core::JointModel * > &	joints_to_halt
	)		const

protected

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

Definition at line 921 of file servo_calcs.cpp.

Here is the call graph for this function:

◆ twistStampedCB()

void moveit_servo::ServoCalcs::twistStampedCB ( const geometry_msgs::msg::TwistStamped::ConstSharedPtr & msg )

protected

Definition at line 1154 of file servo_calcs.cpp.

Here is the call graph for this function:

◆ updateJoints()

void moveit_servo::ServoCalcs::updateJoints ( )

protected

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

Definition at line 937 of file servo_calcs.cpp.

Here is the caller graph for this function:

Member Data Documentation

◆ collision_velocity_scale_

double moveit_servo::ServoCalcs::collision_velocity_scale_ = 1.0

protected

Definition at line 324 of file servo_calcs.h.

◆ collision_velocity_scale_sub_

rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr moveit_servo::ServoCalcs::collision_velocity_scale_sub_

protected

Definition at line 305 of file servo_calcs.h.

◆ control_dimensions_

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

protected

Definition at line 337 of file servo_calcs.h.

◆ control_dimensions_server_

rclcpp::Service<moveit_msgs::srv::ChangeControlDimensions>::SharedPtr moveit_servo::ServoCalcs::control_dimensions_server_

protected

Definition at line 309 of file servo_calcs.h.

◆ current_state_

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

protected

Definition at line 287 of file servo_calcs.h.

◆ delta_theta_

Eigen::ArrayXd moveit_servo::ServoCalcs::delta_theta_

protected

Definition at line 327 of file servo_calcs.h.

◆ done_stopping_

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

protected

Definition at line 316 of file servo_calcs.h.

◆ drift_dimensions_

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

protected

Definition at line 334 of file servo_calcs.h.

◆ drift_dimensions_server_

rclcpp::Service<moveit_msgs::srv::ChangeDriftDimensions>::SharedPtr moveit_servo::ServoCalcs::drift_dimensions_server_

protected

Definition at line 310 of file servo_calcs.h.

◆ gazebo_redundant_message_count_

const int moveit_servo::ServoCalcs::gazebo_redundant_message_count_ = 30

protected

Definition at line 329 of file servo_calcs.h.

◆ have_nonzero_command_

bool moveit_servo::ServoCalcs::have_nonzero_command_ = false

protected

Definition at line 279 of file servo_calcs.h.

◆ have_nonzero_joint_command_

bool moveit_servo::ServoCalcs::have_nonzero_joint_command_ = false

protected

Definition at line 278 of file servo_calcs.h.

◆ have_nonzero_twist_stamped_

bool moveit_servo::ServoCalcs::have_nonzero_twist_stamped_ = false

protected

Definition at line 277 of file servo_calcs.h.

◆ ik_base_to_tip_frame_

Eigen::Isometry3d moveit_servo::ServoCalcs::ik_base_to_tip_frame_

protected

Definition at line 362 of file servo_calcs.h.

◆ ik_solver_

kinematics::KinematicsBaseConstPtr moveit_servo::ServoCalcs::ik_solver_

protected

Definition at line 361 of file servo_calcs.h.

◆ input_cv_

std::condition_variable moveit_servo::ServoCalcs::input_cv_

protected

Definition at line 351 of file servo_calcs.h.

◆ internal_joint_state_

sensor_msgs::msg::JointState moveit_servo::ServoCalcs::internal_joint_state_

protected

Definition at line 293 of file servo_calcs.h.

◆ joint_cmd_sub_

rclcpp::Subscription<control_msgs::msg::JointJog>::SharedPtr moveit_servo::ServoCalcs::joint_cmd_sub_

protected

Definition at line 304 of file servo_calcs.h.

◆ joint_command_is_stale_

bool moveit_servo::ServoCalcs::joint_command_is_stale_ = false

protected

Definition at line 322 of file servo_calcs.h.

◆ joint_model_group_

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

protected

Definition at line 285 of file servo_calcs.h.

◆ joint_servo_cmd_

control_msgs::msg::JointJog moveit_servo::ServoCalcs::joint_servo_cmd_

protected

Definition at line 283 of file servo_calcs.h.

◆ joint_state_name_map_

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

protected

Definition at line 294 of file servo_calcs.h.

◆ joint_state_sub_

rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr moveit_servo::ServoCalcs::joint_state_sub_

protected

Definition at line 302 of file servo_calcs.h.

◆ last_sent_command_

trajectory_msgs::msg::JointTrajectory::SharedPtr moveit_servo::ServoCalcs::last_sent_command_

protected

Definition at line 299 of file servo_calcs.h.

◆ latest_joint_cmd_

control_msgs::msg::JointJog::ConstSharedPtr moveit_servo::ServoCalcs::latest_joint_cmd_

protected

Definition at line 344 of file servo_calcs.h.

◆ latest_joint_cmd_is_nonzero_

bool moveit_servo::ServoCalcs::latest_joint_cmd_is_nonzero_ = false

protected

Definition at line 348 of file servo_calcs.h.

◆ latest_joint_command_stamp_

rclcpp::Time moveit_servo::ServoCalcs::latest_joint_command_stamp_ = rclcpp::Time(0., RCL_ROS_TIME)

protected

Definition at line 346 of file servo_calcs.h.

◆ latest_twist_cmd_is_nonzero_

bool moveit_servo::ServoCalcs::latest_twist_cmd_is_nonzero_ = false

protected

Definition at line 347 of file servo_calcs.h.

◆ latest_twist_command_stamp_

rclcpp::Time moveit_servo::ServoCalcs::latest_twist_command_stamp_ = rclcpp::Time(0., RCL_ROS_TIME)

protected

Definition at line 345 of file servo_calcs.h.

◆ latest_twist_stamped_

geometry_msgs::msg::TwistStamped::ConstSharedPtr moveit_servo::ServoCalcs::latest_twist_stamped_

protected

Definition at line 343 of file servo_calcs.h.

◆ main_loop_mutex_

std::mutex moveit_servo::ServoCalcs::main_loop_mutex_

mutableprotected

Definition at line 340 of file servo_calcs.h.

◆ multiarray_outgoing_cmd_pub_

rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr moveit_servo::ServoCalcs::multiarray_outgoing_cmd_pub_

protected

Definition at line 308 of file servo_calcs.h.

◆ new_input_cmd_

bool moveit_servo::ServoCalcs::new_input_cmd_ = false

protected

Definition at line 352 of file servo_calcs.h.

◆ node_

std::shared_ptr<rclcpp::Node> moveit_servo::ServoCalcs::node_

protected

Definition at line 258 of file servo_calcs.h.

◆ num_joints_

unsigned int moveit_servo::ServoCalcs::num_joints_

protected

Definition at line 331 of file servo_calcs.h.

◆ ok_to_publish_

bool moveit_servo::ServoCalcs::ok_to_publish_ = false

protected

Definition at line 323 of file servo_calcs.h.

◆ original_joint_state_

sensor_msgs::msg::JointState moveit_servo::ServoCalcs::original_joint_state_

protected

Definition at line 293 of file servo_calcs.h.

◆ parameters_

const std::shared_ptr<const moveit_servo::ServoParameters> moveit_servo::ServoCalcs::parameters_

protected

Definition at line 261 of file servo_calcs.h.

◆ paused_

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

protected

Definition at line 320 of file servo_calcs.h.

◆ planning_scene_monitor_

planning_scene_monitor::PlanningSceneMonitorPtr moveit_servo::ServoCalcs::planning_scene_monitor_

protected

Definition at line 264 of file servo_calcs.h.

◆ reset_servo_status_

rclcpp::Service<std_srvs::srv::Empty>::SharedPtr moveit_servo::ServoCalcs::reset_servo_status_

protected

Definition at line 311 of file servo_calcs.h.

◆ robot_link_command_frame_

std::string moveit_servo::ServoCalcs::robot_link_command_frame_

protected

Definition at line 355 of file servo_calcs.h.

◆ smoother_

std::shared_ptr<online_signal_smoothing::SmoothingBaseClass> moveit_servo::ServoCalcs::smoother_

protected

Definition at line 297 of file servo_calcs.h.

◆ smoothing_loader_

pluginlib::ClassLoader<online_signal_smoothing::SmoothingBaseClass> moveit_servo::ServoCalcs::smoothing_loader_

protected

Definition at line 359 of file servo_calcs.h.

◆ status_

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

protected

Definition at line 319 of file servo_calcs.h.

◆ status_pub_

rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr moveit_servo::ServoCalcs::status_pub_

protected

Definition at line 306 of file servo_calcs.h.

◆ stop_requested_

bool moveit_servo::ServoCalcs::stop_requested_

protected

Definition at line 315 of file servo_calcs.h.

◆ tf_moveit_to_ee_frame_

Eigen::Isometry3d moveit_servo::ServoCalcs::tf_moveit_to_ee_frame_

protected

Definition at line 342 of file servo_calcs.h.

◆ tf_moveit_to_robot_cmd_frame_

Eigen::Isometry3d moveit_servo::ServoCalcs::tf_moveit_to_robot_cmd_frame_

protected

Definition at line 341 of file servo_calcs.h.

◆ thread_

std::thread moveit_servo::ServoCalcs::thread_

protected

Definition at line 314 of file servo_calcs.h.

◆ trajectory_outgoing_cmd_pub_

rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr moveit_servo::ServoCalcs::trajectory_outgoing_cmd_pub_

protected

Definition at line 307 of file servo_calcs.h.

◆ twist_command_is_stale_

bool moveit_servo::ServoCalcs::twist_command_is_stale_ = false

protected

Definition at line 321 of file servo_calcs.h.

◆ twist_stamped_cmd_

geometry_msgs::msg::TwistStamped moveit_servo::ServoCalcs::twist_stamped_cmd_

protected

Definition at line 282 of file servo_calcs.h.

◆ twist_stamped_sub_

rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr moveit_servo::ServoCalcs::twist_stamped_sub_

protected

Definition at line 303 of file servo_calcs.h.

◆ updated_filters_

bool moveit_servo::ServoCalcs::updated_filters_ = false

protected

Definition at line 274 of file servo_calcs.h.

◆ use_inv_jacobian_

bool moveit_servo::ServoCalcs::use_inv_jacobian_ = false

protected

Definition at line 363 of file servo_calcs.h.

◆ wait_for_servo_commands_

bool moveit_servo::ServoCalcs::wait_for_servo_commands_ = true

protected

Definition at line 271 of file servo_calcs.h.

◆ zero_velocity_count_

int moveit_servo::ServoCalcs::zero_velocity_count_ = 0

protected

Definition at line 268 of file servo_calcs.h.

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

moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h
moveit_ros/moveit_servo/src/servo_calcs.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ServoCalcs()

◆ ~ServoCalcs()

Member Function Documentation

◆ applyJointUpdate()

◆ calculateSingleIteration()

◆ cartesianServoCalcs()

◆ changeControlDimensions()

◆ changeDriftDimensions()

◆ checkValidCommand() [1/2]

◆ checkValidCommand() [2/2]

◆ collisionVelocityScaleCB()

◆ composeJointTrajMessage()

◆ enforceControlDimensions()

◆ enforcePositionLimits()

◆ filteredHalt()

◆ getCommandFrameTransform() [1/2]

◆ getCommandFrameTransform() [2/2]

◆ getEEFrameTransform() [1/2]

◆ getEEFrameTransform() [2/2]

◆ insertRedundantPointsIntoTrajectory()

◆ internalServoUpdate()

◆ jointCmdCB()

◆ jointServoCalcs()

◆ jointStateCB()

◆ mainCalcLoop()

◆ removeDimension()

◆ removeDriftDimensions()

◆ resetLowPassFilters()

◆ resetServoStatus()

◆ robotLinkCommandFrameCallback()

◆ scaleCartesianCommand()

◆ scaleJointCommand()

◆ setPaused()

◆ start()

◆ stop()

◆ suddenHalt()

◆ twistStampedCB()

◆ updateJoints()

Member Data Documentation

◆ collision_velocity_scale_

◆ collision_velocity_scale_sub_

◆ control_dimensions_

◆ control_dimensions_server_

◆ current_state_

◆ delta_theta_

◆ done_stopping_

◆ drift_dimensions_

◆ drift_dimensions_server_

◆ gazebo_redundant_message_count_

◆ have_nonzero_command_

◆ have_nonzero_joint_command_

◆ have_nonzero_twist_stamped_

◆ ik_base_to_tip_frame_

◆ ik_solver_

◆ input_cv_

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

◆ latest_joint_command_stamp_

◆ latest_twist_cmd_is_nonzero_

◆ latest_twist_command_stamp_

◆ latest_twist_stamped_

◆ main_loop_mutex_

◆ multiarray_outgoing_cmd_pub_

◆ new_input_cmd_

◆ node_

◆ num_joints_

◆ ok_to_publish_