Robot arm control interface. More...

#include <control_base.hpp>

Inheritance diagram for ArmControlBase:

Collaboration diagram for ArmControlBase:

Public Member Functions
	ArmControlBase (const std::string node_name, const rclcpp::NodeOptions &options)
	Constructor of class ArmControlBase. More...

virtual	~ArmControlBase ()
	Default destructor of class ArmControlBase.

virtual bool	moveToTcpPose (double x, double y, double z, double alpha, double beta, double gamma, double vel, double acc)=0
	Move the robot end-effector to a goal pose (position and orientation) w.r.t the robot base in 3D Cartesian space. More...

virtual bool	moveToTcpPose (const Eigen::Isometry3d &pose, double vel, double acc)
	Move the robot end-effector to a goal pose (position and orientation) w.r.t the robot base in 3D Cartesian space. More...

virtual bool	moveToTcpPose (const geometry_msgs::msg::PoseStamped &pose_stamped, double vel, double acc)
	Move the robot end-effector to a goal pose (position and orientation) w.r.t the robot base in 3D Cartesian space. More...

virtual bool	moveToJointValues (const std::vector< double > &joint_values, double vel, double acc)=0
	Move the robot to a joint value goal. More...

virtual bool	open (const double distance=0)=0
	Open the robot gripper and make it ready for grasping. More...

virtual bool	close (const double distance=0)=0
	Close the robot gripper and let it grasp an object. More...

virtual bool	pick (double x, double y, double z, double alpha, double beta, double gamma, double vel, double acc, double vel_scale, double approach)
	Make the robot arm to pick an object from a grasp pose w.r.t the robot base. More...

virtual bool	pick (const geometry_msgs::msg::PoseStamped &pose_stamped, double vel, double acc, double vel_scale, double approach)
	Make the robot arm to pick an object from a grasp pose w.r.t the robot base. More...

virtual bool	place (double x, double y, double z, double alpha, double beta, double gamma, double vel, double acc, double vel_scale, double retract)
	Make the robot arm to place an object from a place pose w.r.t the robot base. More...

virtual bool	place (const geometry_msgs::msg::PoseStamped &pose_stamped, double vel, double acc, double vel_scale, double retract)
	Make the robot arm to place an object from a place pose w.r.t the robot base. More...

void	toTcpPose (const geometry_msgs::msg::PoseStamped &pose_stamped, TcpPose &tcp_pose)
	Convert geometry_msgs::msg::PoseStamped to TcpPose. More...

void	toTcpPose (const Eigen::Isometry3d &pose, TcpPose &tcp_pose)
	Convert Eigen::Isometry3d to TcpPose. More...

virtual bool	checkTcpGoalArrived (Eigen::Isometry3d &tcp_goal)
	Function to check if the end-effector arrived the goal pose. More...

virtual bool	checkJointValueGoalArrived (const std::vector< double > &joint_goal)
	Function to check if the robot arm arrived the joint value goal. More...

virtual void	parseArgs ()=0
	Parse arguments. More...

virtual bool	startLoop ()=0
	Start control loop. More...

virtual void	publishTFGoal ()
	Publish tf_msg_.

void	updateTFGoal (const geometry_msgs::msg::PoseStamped &pose_stamped)
	Update tf_msg_. More...

Eigen::Vector3d	getUnitApproachVector (const double &alpha, const double &beta, const double &gamma)
	This function is used to rotate a unit vector along z axis, i.e. (0, 0, 1) by the assigned rpy euler angles. More...

Protected Attributes
rclcpp::Publisher< sensor_msgs::msg::JointState >::SharedPtr	joint_pub_
	Joint state publisher.

std::vector< std::string >	joint_names_
	Joint names.

TcpPose	tcp_pose_
	Current end-effctor pose.

std::vector< double >	joint_values_
	Current joint value.

std::mutex	m_
	Mutex to guard the tcp_pose_ usage.

double	time_out_
	Time duration to finish a pick or place task.

std::thread	tf_thread_
	Thread to publish tf pose.

geometry_msgs::msg::TransformStamped	tf_msg_
	TF message converted from the pose stamped input to the move or pick/place commands.

tf2_ros::StaticTransformBroadcaster	broadcaster_
	TF broadcaster.

Detailed Description

Robot arm control interface.

Constructor & Destructor Documentation

◆ ArmControlBase()

ArmControlBase::ArmControlBase	(	const std::string	node_name,
		const rclcpp::NodeOptions &	options
	)

inline

Constructor of class ArmControlBase.

Parameters

node_name	The name of the ROS2 node.
options	ROS2 node options.

Member Function Documentation

◆ checkJointValueGoalArrived()

bool ArmControlBase::checkJointValueGoalArrived ( const std::vector< double > & joint_goal )

virtual

Function to check if the robot arm arrived the joint value goal.

Parameters

joint_values Joint value goal of the robot arm.

Returns: If the robot arrived the joint value goal within a time_out_ duration, return true. Otherwise, return false.

◆ checkTcpGoalArrived()

bool ArmControlBase::checkTcpGoalArrived ( Eigen::Isometry3d & tcp_goal )

virtual

Function to check if the end-effector arrived the goal pose.

Parameters

tcp_goal Goal pose of the end-effector.

Returns: If the end-effector arrived the goal pose within a time_out_ duration, return true. Otherwise, return false.

◆ close()

virtual bool ArmControlBase::close ( const double distance = 0 )

pure virtual

Close the robot gripper and let it grasp an object.

Parameters

distance How large the fingers of the gripper close.

Returns: If the robot successfully receives the "close" command, return true. Otherwise, return false.

◆ getUnitApproachVector()

Eigen::Vector3d ArmControlBase::getUnitApproachVector	(	const double &	alpha,
		const double &	beta,
		const double &	gamma
	)

This function is used to rotate a unit vector along z axis, i.e. (0, 0, 1) by the assigned rpy euler angles.

Parameters

alpha	Rotation euler angle around X axis.
beta	Rotation euler angle around Y axis.
gamma	Rotation euler angle around Z axis.

◆ moveToJointValues()

virtual bool ArmControlBase::moveToJointValues	(	const std::vector< double > &	joint_values,
		double	vel,
		double	acc
	)

pure virtual

Move the robot to a joint value goal.

Parameters

joint_values	Goal joint values, the number of joints depends on the robot arm model.
vel	Max joint velocity.
acc	Max joint acceleration.

Returns: If the robot successfully receives the "move" command, return True. Otherwise, return false.

◆ moveToTcpPose() [1/3]

virtual bool ArmControlBase::moveToTcpPose	(	double	x,
		double	y,
		double	z,
		double	alpha,
		double	beta,
		double	gamma,
		double	vel,
		double	acc
	)

pure virtual

Move the robot end-effector to a goal pose (position and orientation) w.r.t the robot base in 3D Cartesian space.

Parameters

x	Goal position on X dimension.
y	Goal position on Y dimension.
z	Goal position on Z dimension.
alpha	Goal rotation euler angle along X axis.
beta	Goal rotation euler angle along Y axis.
gamma	Goal rotation euler angle along Z axis.
vel	Max joint velocity.
acc	Max joint acceleration.

Returns: If the robot successfully receives the "move" command, return True. Otherwise, return false.

◆ moveToTcpPose() [2/3]

bool ArmControlBase::moveToTcpPose	(	const Eigen::Isometry3d &	pose,
		double	vel,
		double	acc
	)

virtual

Move the robot end-effector to a goal pose (position and orientation) w.r.t the robot base in 3D Cartesian space.

Parameters

pose	Goal pose as a Eigen transform (Isometry3d).
vel	Max joint velocity.
acc	Max joint acceleration.

Returns: If the robot successfully receives the "move" command, return True. Otherwise, return false.

◆ moveToTcpPose() [3/3]

bool ArmControlBase::moveToTcpPose	(	const geometry_msgs::msg::PoseStamped &	pose_stamped,
		double	vel,
		double	acc
	)

virtual

Move the robot end-effector to a goal pose (position and orientation) w.r.t the robot base in 3D Cartesian space.

Parameters

pose_stamped	Goal pose as geometry_msgs/PoseStamped.
vel	Max joint velocity.
acc	Max joint acceleration.

Returns: If the robot successfully receives the "move" command, return True. Otherwise, return false.

◆ open()

virtual bool ArmControlBase::open ( const double distance = 0 )

pure virtual

Open the robot gripper and make it ready for grasping.

Parameters

distance How large the fingers of the gripper open.

Returns: If the robot successfully receives the "open" command, return true. Otherwise, return false.

◆ parseArgs()

virtual void ArmControlBase::parseArgs ( )

pure virtual

Parse arguments.

This function is used to parse the communication or control configuration parameters. A common method is defining the configuration parameters in a .yaml file, then loading them as ROS2 node parameters and parsing them by ROS2 node parameter client.

◆ pick() [1/2]

bool ArmControlBase::pick	(	double	x,
		double	y,
		double	z,
		double	alpha,
		double	beta,
		double	gamma,
		double	vel,
		double	acc,
		double	vel_scale,
		double	approach
	)

virtual

Make the robot arm to pick an object from a grasp pose w.r.t the robot base.

This function defines a sequence of motions:

Move the end-effector to a pose above the object.
Open gripper.
Stretch the end-effector along its Z axis to the grasp pose that gripper can grasp the object.
Close gripper.
Move the end-effector back to the pose above the object.

Parameters

x	Position of grasp pose on X dimension.
y	Position of grasp pose on Y dimension.
z	Position of grasp pose on Z dimension.
alpha	Rotation euler angle of grasp pose along X axis.
beta	Rotation euler angle of grasp pose along Y axis.
gamma	Rotation euler angle of grasp pose along Z axis.
vel	Max joint velocity.
acc	Max joint acceleration.
vel_scale	Scale factor to slow down the end-effector velocity, when it stretch to or move back from the grasp pose.
approach	The stretch distance.

Returns: If the robot successfully finishes the "pick" motions, return True. Otherwise, return false.

Note: The grasp pose should have Z axis point out from the end-effector link.

◆ pick() [2/2]

bool ArmControlBase::pick	(	const geometry_msgs::msg::PoseStamped &	pose_stamped,
		double	vel,
		double	acc,
		double	vel_scale,
		double	approach
	)

virtual

Make the robot arm to pick an object from a grasp pose w.r.t the robot base.

This function defines a sequence of motions:

Move the end-effector to a pose above the object.
Open gripper.
Stretch the end-effector along its Z axis to the grasp pose that gripper can grasp the object.
Close gripper.
Move the end-effector back to the pose above the object.

Parameters

pose_stamped	Pose received from the grasp planning algorithm. See also https://github.com/intel/ros2_grasp_library.
vel	Max joint velocity.
acc	Max joint acceleration.
vel_scale	Scale factor to slow down the end-effector velocity, when it stretch to or move back from the grasp pose.
approach	The stretch distance.

Returns: If the robot successfully finishes the "pick" motions, return True. Otherwise, return false.

Note: The grasp pose should have Z axis point out from the end-effector link.

◆ place() [1/2]

bool ArmControlBase::place	(	double	x,
		double	y,
		double	z,
		double	alpha,
		double	beta,
		double	gamma,
		double	vel,
		double	acc,
		double	vel_scale,
		double	retract
	)

virtual

Make the robot arm to place an object from a place pose w.r.t the robot base.

This function defines a sequence of motions:

Move the end-effector to a pre-place pose.
Stretch the end-effector along its Z axis to the place pose.
Open gripper.
Move the end-effector back to the pre-place pose.

Parameters

x	Position of place pose on X dimension.
y	Position of place pose on Y dimension.
z	Position of place pose on Z dimension.
alpha	Rotation euler angle of place pose along X axis.
beta	Rotation euler angle of place pose along Y axis.
gamma	Rotation euler angle of place pose along Z axis.
vel	Max joint velocity.
acc	Max joint acceleration.
vel_scale	Scale factor to slow down the end-effector velocity, when it stretch to or move back from the place pose.
retract	The retract distance from the place pose.

Returns: If the robot successfully finishes the "place" motions, return True. Otherwise, return false.

Note: The place pose should have Z axis point out from the end-effector link.

◆ place() [2/2]

bool ArmControlBase::place	(	const geometry_msgs::msg::PoseStamped &	pose_stamped,
		double	vel,
		double	acc,
		double	vel_scale,
		double	retract
	)

virtual

Make the robot arm to place an object from a place pose w.r.t the robot base.

This function defines a sequence of motions:

Move the end-effector to a pre-place pose.
Stretch the end-effector along its Z axis to the place pose.
Open gripper.
Move the end-effector back to the pre-place pose.

Parameters

pose_stamped	Pose of the end-effector to place an object.
vel	Max joint velocity.
acc	Max joint acceleration.
vel_scale	Scale factor to slow down the end-effector velocity, when it stretch to or retract back from the place pose.
retract	The retract distance from the place pose.

Returns: If the robot successfully finishes the "place" motions, return True. Otherwise, return false.

Note: The place pose should have Z axis point out from the end-effector link.

◆ startLoop()

virtual bool ArmControlBase::startLoop ( )

pure virtual

Start control loop.

This function is used to initialize the communication process and start the thread that reads and publishes the robot state.

◆ toTcpPose() [1/2]

void ArmControlBase::toTcpPose	(	const geometry_msgs::msg::PoseStamped &	pose_stamped,
		TcpPose &	tcp_pose
	)

Convert geometry_msgs::msg::PoseStamped to TcpPose.

Parameters

pose_stamped	Pose of the end-effector.
tcp_pose	Variable to store the converted result.

◆ toTcpPose() [2/2]

void ArmControlBase::toTcpPose	(	const Eigen::Isometry3d &	pose,
		TcpPose &	tcp_pose
	)

Convert Eigen::Isometry3d to TcpPose.

Parameters

pose	Pose of the end-effector.
tcp_pose	Variable to store the converted result.

◆ updateTFGoal()

void ArmControlBase::updateTFGoal ( const geometry_msgs::msg::PoseStamped & pose_stamped )

Update tf_msg_.

Parameters

pose_stamped Pose goal input to the move or pick/place commands.

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

include/robot_interface/control_base.hpp
src/control_base.cpp

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ArmControlBase()

Member Function Documentation

◆ checkJointValueGoalArrived()

◆ checkTcpGoalArrived()

◆ close()

◆ getUnitApproachVector()

◆ moveToJointValues()

◆ moveToTcpPose() [1/3]

◆ moveToTcpPose() [2/3]

◆ moveToTcpPose() [3/3]

◆ open()

◆ parseArgs()

◆ pick() [1/2]

◆ pick() [2/2]

◆ place() [1/2]

◆ place() [2/2]

◆ startLoop()

◆ toTcpPose() [1/2]

◆ toTcpPose() [2/2]

◆ updateTFGoal()