#include <jog_calcs.h>

Public Member Functions
bool	isInitialized ()
	Check if the robot state is being updated and the end effector transform is known. More...

	JogCalcs (const JogArmParameters &parameters, const robot_model_loader::RobotModelLoaderPtr &model_loader_ptr)

void	startMainLoop (JogArmShared &shared_variables)

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

void	applyVelocityScaling (JogArmShared &shared_variables, 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...

bool	cartesianJogCalcs (geometry_msgs::TwistStamped &cmd, JogArmShared &shared_variables)
	Do jogging calculations for Cartesian twist commands. More...

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

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

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

bool	enforceSRDFPositionLimits (trajectory_msgs::JointTrajectory &new_joint_traj)
	Avoid overshooting joint limits. More...

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

bool	jointJogCalcs (const control_msgs::JointJog &cmd, JogArmShared &shared_variables)
	Do jogging calculations for direct commands to a joint. More...

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

void	publishStatus () const
	Publish the status of the jogger to a ROS topic. More...

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

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	suddenHalt (trajectory_msgs::JointTrajectory &joint_traj)
	Suddenly halt for a joint limit or other critical issue. Is handled differently for position vs. velocity control. More...

void	suddenHalt (Eigen::ArrayXd &delta_theta)

void	updateCachedStatus (JogArmShared &shared_variables)
	Update the stashed status so it can be retrieved asynchronously. More...

bool	updateJoints (JogArmShared &shared_variables)
	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 &jacobian, const Eigen::MatrixXd &pseudo_inverse)
	Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion. More...

Protected Attributes
ros::Rate	default_sleep_rate_

Eigen::ArrayXd	delta_theta_

const int	gazebo_redundant_message_count_ = 30

sensor_msgs::JointState	incoming_joint_state_

sensor_msgs::JointState	internal_joint_state_

std::atomic< bool >	is_initialized_

const moveit::core::JointModelGroup *	joint_model_group_

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

moveit::core::RobotStatePtr	kinematic_state_

ros::NodeHandle	nh_

uint	num_joints_

sensor_msgs::JointState	original_joint_state_

trajectory_msgs::JointTrajectory	outgoing_command_

JogArmParameters	parameters_

std::vector< LowPassFilter >	position_filters_

Eigen::ArrayXd	prev_joint_velocity_

StatusCode	status_ = NO_WARNING

ros::Publisher	status_pub_

Eigen::Isometry3d	tf_moveit_to_cmd_frame_

Detailed Description

Definition at line 56 of file jog_calcs.h.

Constructor & Destructor Documentation

◆ JogCalcs()

moveit_jog_arm::JogCalcs::JogCalcs	(	const JogArmParameters &	parameters,
		const robot_model_loader::RobotModelLoaderPtr &	model_loader_ptr
	)

Definition at line 47 of file jog_calcs.cpp.

Member Function Documentation

◆ addJointIncrements()

bool moveit_jog_arm::JogCalcs::addJointIncrements	(	sensor_msgs::JointState &	output,
		const Eigen::VectorXd &	increments
	)		const

protected

Definition at line 815 of file jog_calcs.cpp.

◆ applyVelocityScaling()

void moveit_jog_arm::JogCalcs::applyVelocityScaling	(	JogArmShared &	shared_variables,
		Eigen::ArrayXd &	delta_theta,
		double	singularity_scale
	)

protected

Slow motion down if close to singularity or collision.

Parameters

shared_variables	data shared between threads, tells how close we are to collision
delta_theta	motion command, used in calculating new_joint_tray
singularity_scale	tells how close we are to a singularity

Definition at line 477 of file jog_calcs.cpp.

◆ calculateJointVelocities()

void moveit_jog_arm::JogCalcs::calculateJointVelocities	(	sensor_msgs::JointState &	joint_state,
		const Eigen::ArrayXd &	delta_theta
	)

protected

Convert an incremental position command to joint velocity message.

Definition at line 441 of file jog_calcs.cpp.

◆ cartesianJogCalcs()

bool moveit_jog_arm::JogCalcs::cartesianJogCalcs	(	geometry_msgs::TwistStamped &	cmd,
		JogArmShared &	shared_variables
	)

protected

Do jogging calculations for Cartesian twist commands.

Definition at line 253 of file jog_calcs.cpp.

◆ composeJointTrajMessage()

trajectory_msgs::JointTrajectory moveit_jog_arm::JogCalcs::composeJointTrajMessage ( sensor_msgs::JointState & joint_state ) const

protected

Compose the outgoing JointTrajectory message.

Definition at line 449 of file jog_calcs.cpp.

◆ convertDeltasToOutgoingCmd()

bool moveit_jog_arm::JogCalcs::convertDeltasToOutgoingCmd ( )

protected

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

Definition at line 391 of file jog_calcs.cpp.

◆ enforceSRDFAccelVelLimits()

void moveit_jog_arm::JogCalcs::enforceSRDFAccelVelLimits ( Eigen::ArrayXd & delta_theta )

protected

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

Definition at line 565 of file jog_calcs.cpp.

◆ enforceSRDFPositionLimits()

bool moveit_jog_arm::JogCalcs::enforceSRDFPositionLimits ( trajectory_msgs::JointTrajectory & new_joint_traj )

protected

Avoid overshooting joint limits.

Definition at line 639 of file jog_calcs.cpp.

◆ insertRedundantPointsIntoTrajectory()

void moveit_jog_arm::JogCalcs::insertRedundantPointsIntoTrajectory	(	trajectory_msgs::JointTrajectory &	trajectory,
		int	count
	)		const

protected

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

Definition at line 422 of file jog_calcs.cpp.

◆ isInitialized()

bool moveit_jog_arm::JogCalcs::isInitialized ( )

Check if the robot state is being updated and the end effector transform is known.

Returns: true if initialized properly

Definition at line 247 of file jog_calcs.cpp.

◆ jointJogCalcs()

bool moveit_jog_arm::JogCalcs::jointJogCalcs	(	const control_msgs::JointJog &	cmd,
		JogArmShared &	shared_variables
	)

protected

Do jogging calculations for direct commands to a joint.

Definition at line 357 of file jog_calcs.cpp.

◆ lowPassFilterPositions()

void moveit_jog_arm::JogCalcs::lowPassFilterPositions ( sensor_msgs::JointState & joint_state )

protected

Smooth position commands with a lowpass filter.

Definition at line 433 of file jog_calcs.cpp.

◆ publishStatus()

void moveit_jog_arm::JogCalcs::publishStatus ( ) const

protected

Publish the status of the jogger to a ROS topic.

Definition at line 678 of file jog_calcs.cpp.

◆ removeDimension()

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

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 834 of file jog_calcs.cpp.

◆ scaleCartesianCommand()

Eigen::VectorXd moveit_jog_arm::JogCalcs::scaleCartesianCommand ( const geometry_msgs::TwistStamped & command ) const

protected

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 750 of file jog_calcs.cpp.

◆ scaleJointCommand()

Eigen::VectorXd moveit_jog_arm::JogCalcs::scaleJointCommand ( const control_msgs::JointJog & command ) const

protected

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 780 of file jog_calcs.cpp.

◆ startMainLoop()

void moveit_jog_arm::JogCalcs::startMainLoop ( JogArmShared & shared_variables )

Definition at line 67 of file jog_calcs.cpp.

◆ suddenHalt() [1/2]

void moveit_jog_arm::JogCalcs::suddenHalt ( trajectory_msgs::JointTrajectory & joint_traj )

protected

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

Definition at line 694 of file jog_calcs.cpp.

◆ suddenHalt() [2/2]

void moveit_jog_arm::JogCalcs::suddenHalt ( Eigen::ArrayXd & delta_theta )

protected

Definition at line 687 of file jog_calcs.cpp.

◆ updateCachedStatus()

void moveit_jog_arm::JogCalcs::updateCachedStatus ( JogArmShared & shared_variables )

protected

Update the stashed status so it can be retrieved asynchronously.

Definition at line 385 of file jog_calcs.cpp.

◆ updateJoints()

bool moveit_jog_arm::JogCalcs::updateJoints ( JogArmShared & shared_variables )

protected

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

Definition at line 716 of file jog_calcs.cpp.

◆ velocityScalingFactorForSingularity()

double moveit_jog_arm::JogCalcs::velocityScalingFactorForSingularity	(	const Eigen::VectorXd &	commanded_velocity,
		const Eigen::JacobiSVD< Eigen::MatrixXd > &	svd,
		const Eigen::MatrixXd &	jacobian,
		const Eigen::MatrixXd &	pseudo_inverse
	)

protected

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

Definition at line 498 of file jog_calcs.cpp.

Member Data Documentation

◆ default_sleep_rate_

ros::Rate moveit_jog_arm::JogCalcs::default_sleep_rate_

protected

Definition at line 185 of file jog_calcs.h.

◆ delta_theta_

Eigen::ArrayXd moveit_jog_arm::JogCalcs::delta_theta_

protected

Definition at line 176 of file jog_calcs.h.

◆ gazebo_redundant_message_count_

const int moveit_jog_arm::JogCalcs::gazebo_redundant_message_count_ = 30

protected

Definition at line 181 of file jog_calcs.h.

◆ incoming_joint_state_

sensor_msgs::JointState moveit_jog_arm::JogCalcs::incoming_joint_state_

protected

Definition at line 163 of file jog_calcs.h.

◆ internal_joint_state_

sensor_msgs::JointState moveit_jog_arm::JogCalcs::internal_joint_state_

protected

Definition at line 163 of file jog_calcs.h.

◆ is_initialized_

std::atomic<bool> moveit_jog_arm::JogCalcs::is_initialized_

protected

Definition at line 72 of file jog_calcs.h.

◆ joint_model_group_

const moveit::core::JointModelGroup* moveit_jog_arm::JogCalcs::joint_model_group_

protected

Definition at line 156 of file jog_calcs.h.

◆ joint_state_name_map_

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

protected

Definition at line 164 of file jog_calcs.h.

◆ kinematic_state_

moveit::core::RobotStatePtr moveit_jog_arm::JogCalcs::kinematic_state_

protected

Definition at line 158 of file jog_calcs.h.

◆ nh_

ros::NodeHandle moveit_jog_arm::JogCalcs::nh_

protected

Definition at line 69 of file jog_calcs.h.

◆ num_joints_

uint moveit_jog_arm::JogCalcs::num_joints_

protected

Definition at line 183 of file jog_calcs.h.

◆ original_joint_state_

sensor_msgs::JointState moveit_jog_arm::JogCalcs::original_joint_state_

protected

Definition at line 163 of file jog_calcs.h.

◆ outgoing_command_

trajectory_msgs::JointTrajectory moveit_jog_arm::JogCalcs::outgoing_command_

protected

Definition at line 165 of file jog_calcs.h.

◆ parameters_

JogArmParameters moveit_jog_arm::JogCalcs::parameters_

protected

Definition at line 173 of file jog_calcs.h.

◆ position_filters_

std::vector<LowPassFilter> moveit_jog_arm::JogCalcs::position_filters_

protected

Definition at line 167 of file jog_calcs.h.

◆ prev_joint_velocity_

Eigen::ArrayXd moveit_jog_arm::JogCalcs::prev_joint_velocity_

protected

Definition at line 177 of file jog_calcs.h.

◆ status_

StatusCode moveit_jog_arm::JogCalcs::status_ = NO_WARNING

protected

Definition at line 171 of file jog_calcs.h.

◆ status_pub_

ros::Publisher moveit_jog_arm::JogCalcs::status_pub_

protected

Definition at line 169 of file jog_calcs.h.

◆ tf_moveit_to_cmd_frame_

Eigen::Isometry3d moveit_jog_arm::JogCalcs::tf_moveit_to_cmd_frame_

protected

Definition at line 179 of file jog_calcs.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ JogCalcs()

Member Function Documentation

◆ addJointIncrements()

◆ applyVelocityScaling()

◆ calculateJointVelocities()

◆ cartesianJogCalcs()

◆ composeJointTrajMessage()

◆ convertDeltasToOutgoingCmd()

◆ enforceSRDFAccelVelLimits()

◆ enforceSRDFPositionLimits()

◆ insertRedundantPointsIntoTrajectory()

◆ isInitialized()

◆ jointJogCalcs()

◆ lowPassFilterPositions()

◆ publishStatus()

◆ removeDimension()

◆ scaleCartesianCommand()

◆ scaleJointCommand()

◆ startMainLoop()

◆ suddenHalt() [1/2]

◆ suddenHalt() [2/2]

◆ updateCachedStatus()

◆ updateJoints()

◆ velocityScalingFactorForSingularity()

Member Data Documentation

◆ default_sleep_rate_

◆ delta_theta_

◆ gazebo_redundant_message_count_

◆ incoming_joint_state_

◆ internal_joint_state_

◆ is_initialized_

◆ joint_model_group_

◆ joint_state_name_map_

◆ kinematic_state_

◆ nh_

◆ num_joints_

◆ original_joint_state_

◆ outgoing_command_

◆ parameters_

◆ position_filters_

◆ prev_joint_velocity_

◆ status_

◆ status_pub_

◆ tf_moveit_to_cmd_frame_