This controller is made to simplify the usage and the user interaction with the delta device. More...

#include <qb_delta_kinematic_controller.h>

Inheritance diagram for qb_chain_controllers::DeltaKinematicController:

Public Member Functions
	DeltaKinematicController ()
	Start the async spinner and do nothing else. More...

bool	init (hardware_interface::PositionJointInterface *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh) override
	The init function is called to initialize the controller from a non-realtime thread with a pointer to the hardware interface itself, instead of a pointer to a RobotHW. More...

void	starting (const ros::Time &time) override
	This is called from within the realtime thread just before the first call to update. More...

void	stopping (const ros::Time &time) override
	This is called from within the realtime thread just after the last update call before the controller is stopped. More...

void	update (const ros::Time &time, const ros::Duration &period) override
	Update the delta state. More...

	~DeltaKinematicController () override
	Stop the async spinner. More...

Public Member Functions inherited from controller_interface::Controller< hardware_interface::PositionJointInterface >
	Controller ()

virtual bool	init (hardware_interface::PositionJointInterface *, ros::NodeHandle &)

virtual	~Controller ()

Public Member Functions inherited from controller_interface::ControllerBase
	ControllerBase ()

bool	isRunning ()

bool	isRunning ()

bool	startRequest (const ros::Time &time)

bool	startRequest (const ros::Time &time)

bool	stopRequest (const ros::Time &time)

bool	stopRequest (const ros::Time &time)

void	updateRequest (const ros::Time &time, const ros::Duration &period)

void	updateRequest (const ros::Time &time, const ros::Duration &period)

virtual	~ControllerBase ()

Protected Member Functions
void	actionActiveCallback (const std::string &controller)
	Do nothing apart from debug info. More...

void	actionDoneCallback (const actionlib::SimpleClientGoalState &state, const control_msgs::FollowJointTrajectoryResultConstPtr &result, const std::string &controller)
	Restart the waypoint trajectory automatically if waypoints have been retrieved during the initialization. More...

void	actionFeedbackCallback (const control_msgs::FollowJointTrajectoryFeedbackConstPtr &feedback, const std::string &controller)
	Do nothing apart from debug info. More...

void	buildCube (visualization_msgs::InteractiveMarker &interactive_marker)
	Create a cubic marker which can be moved interactively in all the Cartesian space. More...

void	buildEndEffectorControl (visualization_msgs::InteractiveMarker &interactive_marker)
	Add six distinct interactive controls to the given interactive marker. More...

bool	cartesianLinearPlanner (const geometry_msgs::Point &target_pose, std::vector< std::vector< double >> &joint_positions)
	Compute the joint trajectory of all the upper motors from the last computed value to the given target end-effector position in a linearized fashion w.r.t. More...

double	computeDistance (const geometry_msgs::Point &from_pose, const geometry_msgs::Point &to_pose)
	Compute the absolute 3D distance between the given points. More...

std::vector< geometry_msgs::Point >	computeIntermediatePosesTo (const geometry_msgs::Point &target_pose)
	Compute a linear-in-space interpolation from the last computed value to the given target end-effector position. More...

std::vector< std::vector< double > >	computeIntermediateStiffnessesTo (const std::vector< double > &target_joint_stiffnesses, const int &size)
	Compute the linearized trajectory from the last computed value to the given target joint stiffness of all the upper motors. More...

std::map< std::string, trajectory_msgs::JointTrajectory >	computeJointTrajectories (const std::vector< std::vector< double >> &motor_positions, const std::vector< std::vector< double >> &motor_stiffness, const double &target_time_from_start, const double &previous_time_from_start)
	Compute a segment of trajectories for all the upper motors from the given parameters. More...

bool	deltaStatePublisher (const std::vector< double > &motor_joints, const geometry_msgs::Point &ee_pose)
	Compute all the free joint positions (i.e. More...

void	fillMotorJointTrajectories (const std::map< std::string, trajectory_msgs::JointTrajectory > &motor_joint_trajectories)
	Append the given trajectories to the joint trajectories private map. More...

void	filter (const std::vector< double > &b, const std::vector< double > &a, const std::vector< double > &x, std::vector< double > &y)
	Filter data following a discrete parametrization of the samples. More...

void	filterMotorJointTrajectories (const std::vector< double > &b, const std::vector< double > &a)
	Filter all the trajectories stored in the joint trajectories private map. More...

void	filterMotorJointTrajectory (const std::vector< double > &b, const std::vector< double > &a, trajectory_msgs::JointTrajectory &motor_joint_trajectory)
	Filter the given joint trajectory, by considering the whole data as a vector of new measurements (each point is a sample). More...

bool	forwardKinematics (const std::vector< double > &joint_positions, geometry_msgs::Point &ee_pose)
	Compute the forward kinematics of the delta device. More...

std::vector< std::string >	getMotorJointNames (const int &id)

trajectory_msgs::JointTrajectory	getMotorJointTrajectory (const int &id)

std::string	getMotorName (const int &id)

double	getMotorPosition (const int &id)

std::vector< double >	getMotorPositions ()

double	getMotorStiffness (const int &id)

std::vector< double >	getMotorStiffnesses ()

std::vector< double >	getTrajectoryLastPositions ()

std::vector< double >	getTrajectoryLastStiffnesses ()

double	getTrajectoryLastTimeFromStart ()

void	initMarkers ()
	Initialize the `interactive_markers::InteractiveMarkerServer` and the marker displayed in `rviz` to control the end-effector position of the delta device. More...

void	interactiveMarkerCallback (const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
	If waypoints are not used, this become the core method of the class, where commands are computed w.r.t. More...

bool	inverseKinematics (const geometry_msgs::Point &ee_pose, std::vector< double > &joint_positions)
	Compute the inverse kinematics of the delta device. More...

void	move (const trajectory_msgs::JointTrajectory &joint_trajectory, const std::string &motor)
	Build a `control_msgs::FollowJointTrajectoryGoal` with the given joint trajectory and make a call to the Action Server relative to the provided motor controller (using the just created goal). More...

void	move ()
	Make all the calls to the Action Servers relative to all the trajectories previously parsed (either from waypoint or interactive markers). More...

bool	parseVector (const XmlRpc::XmlRpcValue &xml_value, const int &size, std::vector< double > &vector)
	Parse the given `XmlRpcValue` as a `std::vector`, since the `XmlRpc::XmlRpcValue` class does not handle this conversion yet. More...

void	parseWaypointTrajectory (ros::NodeHandle &node_handle)
	Parse all the waypoints set up in the Parameter Server at <robot_namespace>/waypoints. More...

void	startWaypointTrajectory ()
	Parse all the waypoints, filter the whole generated joint trajectory and send the commands to the devices through their relative Action server. More...

void	targetPosesCallback (const geometry_msgs::PointStamped &msg)
	Call the Cartesian Planner to compute the joint trajectory from the current position of the end-effector to the given target. More...

template<class T >
T	xmlCast (XmlRpc::XmlRpcValue xml_value)
	Cast an `XmlRpcValue` from `TypeDouble`, `TypeInt` or `TypeBoolean` to the specified template type. More...

Protected Member Functions inherited from controller_interface::Controller< hardware_interface::PositionJointInterface >
std::string	getHardwareInterfaceType () const

virtual bool	initRequest (hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh, ClaimedResources &claimed_resources)

Protected Attributes
ros::CallbackQueuePtr	callback_queue_

visualization_msgs::InteractiveMarker	controls_

geometry_msgs::Point	ee_offset_

bool	feedback_active_

geometry_msgs::Point	feedback_position_old_

std::vector< double >	filter_param_a_

std::vector< double >	filter_param_b_

bool	filter_trajectory_

ros::Publisher	free_joint_state_publisher_

std::unique_ptr< interactive_markers::InteractiveMarkerServer >	interactive_commands_server_

double	max_displacement_

std::map< std::string, std::unique_ptr< actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > > >	motor_action_clients_

std::map< std::string, std::vector< std::string > >	motor_joint_names_

std::map< std::string, trajectory_msgs::JointTrajectory >	motor_joint_trajectories_

std::map< std::string, hardware_interface::JointHandle >	motor_joints_

std::vector< std::string >	motor_names_

ros::NodeHandle	node_handle_

ros::NodeHandle	node_handle_control_

ros::AsyncSpinner	spinner_

ros::Subscriber	target_poses_sub_

tf2_ros::TransformBroadcaster	tf_broadcaster_

bool	use_interactive_markers_

bool	use_waypoints_

std::string	waypoint_namespace_

ros::WallTimer	waypoint_setup_timer_

Additional Inherited Members
Public Types inherited from controller_interface::ControllerBase
typedef std::vector< hardware_interface::InterfaceResources >	ClaimedResources

Public Attributes inherited from controller_interface::ControllerBase
	CONSTRUCTED

	INITIALIZED

	RUNNING

enum controller_interface::ControllerBase:: { ... }	state_

Detailed Description

This controller is made to simplify the usage and the user interaction with the delta device.

Actually it is an upper level controller which relies on the low-level trajectory controller provided by the qbMoveHW interfaces. This controller also compute the forward kinematics to properly visualize the whole kinematic structure in rviz. Additionally it provides waypoints control mode (similar to the single device and chained mode), and an interactive marker control mode (similar to the one of the single qbmove device).

Definition at line 58 of file qb_delta_kinematic_controller.h.

Constructor & Destructor Documentation

DeltaKinematicController::DeltaKinematicController ( )

Start the async spinner and do nothing else.

The real initialization is done later by init().

See also: init()

Definition at line 32 of file qb_delta_kinematic_controller.cpp.

DeltaKinematicController::~DeltaKinematicController ( )

override

Stop the async spinner.

Definition at line 40 of file qb_delta_kinematic_controller.cpp.

Member Function Documentation

void DeltaKinematicController::actionActiveCallback ( const std::string & controller )

protected

Do nothing apart from debug info.

Parameters

controller The action controller name.

Definition at line 44 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::actionDoneCallback	(	const actionlib::SimpleClientGoalState &	state,
		const control_msgs::FollowJointTrajectoryResultConstPtr &	result,
		const std::string &	controller
	)

protected

Restart the waypoint trajectory automatically if waypoints have been retrieved during the initialization.

Parameters

state	The final state of the action.
result	The error code and error message (`0` if succeeds).
controller	The action controller name.

See also: startWaypointTrajectory()

Definition at line 48 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::actionFeedbackCallback	(	const control_msgs::FollowJointTrajectoryFeedbackConstPtr &	feedback,
		const std::string &	controller
	)

protected

Do nothing apart from debug info.

Parameters

feedback	The action feedback state.
controller	The action controller name.

Definition at line 73 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::buildCube ( visualization_msgs::InteractiveMarker & interactive_marker )

protected

Create a cubic marker which can be moved interactively in all the Cartesian space.

The marker represent the position of the end-effector of the delta.

Parameters

interactive_marker The Interactive Marker structure to be filled.

See also: initMarkers()

Definition at line 79 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::buildEndEffectorControl ( visualization_msgs::InteractiveMarker & interactive_marker )

protected

Add six distinct interactive controls to the given interactive marker.

They let to move the marker in the Cartesian space by limiting the motion to a single principal axis or plane, e.g. only along x axis or in the yz plane.

Parameters

interactive_marker The Interactive Marker structure to be filled.

See also: initMarkers()

Definition at line 98 of file qb_delta_kinematic_controller.cpp.

bool DeltaKinematicController::cartesianLinearPlanner	(	const geometry_msgs::Point &	target_pose,
		std::vector< std::vector< double >> &	joint_positions
	)

protected

Compute the joint trajectory of all the upper motors from the last computed value to the given target end-effector position in a linearized fashion w.r.t.

to the Cartesian space. If there are no last values, current shaft positions of the upper motors are taken.

Parameters

	target_pose	The target end-effector position.
[out]	joint_positions	The vector of all the shaft position of all the upper motors.

Returns: true on success.

See also: computeIntermediatePosesTo(), inverseKinematics()

Definition at line 132 of file qb_delta_kinematic_controller.cpp.

double DeltaKinematicController::computeDistance	(	const geometry_msgs::Point &	from_pose,
		const geometry_msgs::Point &	to_pose
	)

protected

Compute the absolute 3D distance between the given points.

Parameters

from_pose	The point from which to compute the distance.
to_pose	The point to which to compute the distance.

Returns: The computed distance.

Definition at line 154 of file qb_delta_kinematic_controller.cpp.

std::vector< geometry_msgs::Point > DeltaKinematicController::computeIntermediatePosesTo ( const geometry_msgs::Point & target_pose )

protected

Compute a linear-in-space interpolation from the last computed value to the given target end-effector position.

If there are no last values stored, current joint positions are taken to compute the forward kinematics.

Parameters

target_pose The target end-effector position.

Returns: The vector of all the intermediate position of the end-effector.

See also: cartesianLinearPlanner(), computeDistance(), forwardKinematics()

Definition at line 158 of file qb_delta_kinematic_controller.cpp.

std::vector< std::vector< double > > DeltaKinematicController::computeIntermediateStiffnessesTo	(	const std::vector< double > &	target_joint_stiffnesses,
		const int &	size
	)

protected

Compute the linearized trajectory from the last computed value to the given target joint stiffness of all the upper motors.

If there are no last values stored, current stiffness values are taken.

Parameters

target_joint_stiffnesses	The target stiffness value of all the upper motors.
size	The number of points that needs to be generated.

Returns: The vector of all the intermediate stiffness value of all the upper motors.

See also: parseWaypointTrajectory()

Definition at line 180 of file qb_delta_kinematic_controller.cpp.

std::map< std::string, trajectory_msgs::JointTrajectory > DeltaKinematicController::computeJointTrajectories	(	const std::vector< std::vector< double >> &	motor_positions,
		const std::vector< std::vector< double >> &	motor_stiffness,
		const double &	target_time_from_start,
		const double &	previous_time_from_start
	)

protected

Compute a segment of trajectories for all the upper motors from the given parameters.

Note: Be aware that motor_positions must be strictly related to motor_stiffness, i.e. same size and meanings.

Parameters

motor_positions	The vector of shaft positions of all the upper motors.
motor_stiffness	The vector of stiffness values of all the upper motors.
target_time_from_start	The final `time_from_start` of this segment of trajectory.
previous_time_from_start	The initial `time_from_start` of this segment of trajectory.

Returns: The map of joint trajectories of all the upper motors filled with the given segment data.

Definition at line 200 of file qb_delta_kinematic_controller.cpp.

bool DeltaKinematicController::deltaStatePublisher	(	const std::vector< double > &	motor_joints,
		const geometry_msgs::Point &	ee_pose
	)

protected

Compute all the free joint positions (i.e.

of the arms) of the delta device and publish them to the Joint State Publisher. Also update the end-effector floating tf frame with the given end-effector position.

Parameters

motor_joints	The upper motor position.
ee_pose	The end-effector position w.r.t. the upper plate.

Returns: true on success.

See also: update(), forwardKinematics()

Definition at line 222 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::fillMotorJointTrajectories ( const std::map< std::string, trajectory_msgs::JointTrajectory > & motor_joint_trajectories )

protected

Append the given trajectories to the joint trajectories private map.

Parameters

motor_joint_trajectories The map with new joint trajectories (either for the gripper and upper motors).

See also: parseWaypointTrajectory()

Definition at line 282 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::filter	(	const std::vector< double > &	b,
		const std::vector< double > &	a,
		const std::vector< double > &	x,
		std::vector< double > &	y
	)

protected

Filter data following a discrete parametrization of the samples.

Parameters

	b	The vector of numerator coefficients, i.e. the ones applied to the new measurements.
	a	The vector of denominator coefficients, i.e. the ones applied to the old data.
	x	The new measurements.
[out]	y	The whole filtered data.

Definition at line 292 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::filterMotorJointTrajectories	(	const std::vector< double > &	b,
		const std::vector< double > &	a
	)

protected

Filter all the trajectories stored in the joint trajectories private map.

Parameters

b	The vector of numerator coefficients, i.e. the ones applied to the new measurements.
a	The vector of denominator coefficients, i.e. the ones applied to the old data.

See also: filterMotorJointTrajectory()

Definition at line 357 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::filterMotorJointTrajectory	(	const std::vector< double > &	b,
		const std::vector< double > &	a,
		trajectory_msgs::JointTrajectory &	motor_joint_trajectory
	)

protected

Filter the given joint trajectory, by considering the whole data as a vector of new measurements (each point is a sample).

Parameters

	b	The vector of numerator coefficients, i.e. the ones applied to the new measurements.
	a	The vector of denominator coefficients, i.e. the ones applied to the old data.
[out]	motor_joint_trajectory	The joint trajectory to be filtered which is modified in place.

See also: filterMotorJointTrajectories(), filter()

Definition at line 366 of file qb_delta_kinematic_controller.cpp.

bool DeltaKinematicController::forwardKinematics	(	const std::vector< double > &	joint_positions,
		geometry_msgs::Point &	ee_pose
	)

protected

Compute the forward kinematics of the delta device.

Parameters

	joint_positions	The upper motor position.
[out]	ee_pose	The computed end-effector position w.r.t. the upper plate.

Returns: true on success.

See also: deltaStatePublisher(), inverseKinematics()

Definition at line 404 of file qb_delta_kinematic_controller.cpp.

std::vector< std::string > DeltaKinematicController::getMotorJointNames ( const int & id )

protected

Returns: The joint names of the given device ID.

Definition at line 497 of file qb_delta_kinematic_controller.cpp.

trajectory_msgs::JointTrajectory DeltaKinematicController::getMotorJointTrajectory ( const int & id )

protected

Returns: The stored joint trajectory of the given device ID.

Definition at line 501 of file qb_delta_kinematic_controller.cpp.

std::string DeltaKinematicController::getMotorName ( const int & id )

protected

Returns: The device hardware interface name of the given device ID.

Definition at line 477 of file qb_delta_kinematic_controller.cpp.

double DeltaKinematicController::getMotorPosition ( const int & id )

protected

Returns: The shaft position of the given device ID.

Definition at line 481 of file qb_delta_kinematic_controller.cpp.

std::vector< double > DeltaKinematicController::getMotorPositions ( )

protected

Returns: The shaft position of all the upper motors of the delta.

Definition at line 485 of file qb_delta_kinematic_controller.cpp.

double DeltaKinematicController::getMotorStiffness ( const int & id )

protected

Returns: The stiffness value of the given device ID.

Definition at line 489 of file qb_delta_kinematic_controller.cpp.

std::vector< double > DeltaKinematicController::getMotorStiffnesses ( )

protected

Returns: The stiffness value of all the upper motors of the delta.

Definition at line 493 of file qb_delta_kinematic_controller.cpp.

std::vector< double > DeltaKinematicController::getTrajectoryLastPositions ( )

protected

Returns: The last stored shaft position of all the upper motors of the delta.

Definition at line 505 of file qb_delta_kinematic_controller.cpp.

std::vector< double > DeltaKinematicController::getTrajectoryLastStiffnesses ( )

protected

Returns: The last stored stiffness value of all the upper motors of the delta.

Definition at line 512 of file qb_delta_kinematic_controller.cpp.

double DeltaKinematicController::getTrajectoryLastTimeFromStart ( )

protected

Returns: The last stored time_from_start (0 if empty).

Definition at line 519 of file qb_delta_kinematic_controller.cpp.

bool DeltaKinematicController::init	(	hardware_interface::PositionJointInterface *	robot_hw,
		ros::NodeHandle &	root_nh,
		ros::NodeHandle &	controller_nh
	)

overridevirtual

The init function is called to initialize the controller from a non-realtime thread with a pointer to the hardware interface itself, instead of a pointer to a RobotHW.

Parameters

robot_hw	The specific hardware interface used by this controller.
root_nh	A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are setup (publishers, subscribers, services).
controller_nh	A NodeHandle in the namespace from which the controller should read its configuration, and where it should set up its ROS interface.

Returns: true if initialization was successful and the controller is ready to be started.

Reimplemented from controller_interface::Controller< hardware_interface::PositionJointInterface >.

Definition at line 526 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::initMarkers ( )

protected

Initialize the interactive_markers::InteractiveMarkerServer and the marker displayed in rviz to control the end-effector position of the delta device.

This method is called during the controller initialization if required by the user (cannot be started whether waypoints are active).

See also: buildCube(), buildEndEffectorControl(), init()

Definition at line 571 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::interactiveMarkerCallback ( const visualization_msgs::InteractiveMarkerFeedbackConstPtr & feedback )

protected

If waypoints are not used, this become the core method of the class, where commands are computed w.r.t.

the previous state of the interactive marker and the computed joint trajectory is sent to each low level controllers, e.g. the joint trajectory controller of each qbMoveHW.

Parameters

feedback The feedback state of the interactive marker, provided by rviz.

See also: targetPosesCallback()

Definition at line 585 of file qb_delta_kinematic_controller.cpp.

bool DeltaKinematicController::inverseKinematics	(	const geometry_msgs::Point &	ee_pose,
		std::vector< double > &	joint_positions
	)

protected

Compute the inverse kinematics of the delta device.

Parameters

	ee_pose	The end-effector position w.r.t. the upper plate.
[out]	joint_positions	The computed upper motor position.

Returns: true on success.

See also: deltaStatePublisher(), forwardKinematics()

Definition at line 609 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::move	(	const trajectory_msgs::JointTrajectory &	joint_trajectory,
		const std::string &	motor
	)

protected

Build a control_msgs::FollowJointTrajectoryGoal with the given joint trajectory and make a call to the Action Server relative to the provided motor controller (using the just created goal).

Parameters

joint_trajectory	The waypoint trajectory properly filled for all the joints of the controller.
motor	The action controller name.

See also: move()

Definition at line 648 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::move ( )

protected

Make all the calls to the Action Servers relative to all the trajectories previously parsed (either from waypoint or interactive markers).

See also: move(const trajectory_msgs::JointTrajectory &, const std::string &)

Definition at line 642 of file qb_delta_kinematic_controller.cpp.

bool DeltaKinematicController::parseVector	(	const XmlRpc::XmlRpcValue &	xml_value,
		const int &	size,
		std::vector< double > &	vector
	)

protected

Parse the given XmlRpcValue as a std::vector, since the XmlRpc::XmlRpcValue class does not handle this conversion yet.

Parameters

	xml_value	The value retrieved from the Parameter Server to be parsed as a `std::vector`.
	controller	The action controller name.
[out]	vector	The `std::vector` parsed from the `XmlRpcValue`.

Returns: true on success.

See also: parseWaypointTrajectory(), xmlCast()

Definition at line 657 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::parseWaypointTrajectory ( ros::NodeHandle & node_handle )

protected

Parse all the waypoints set up in the Parameter Server at <robot_namespace>/waypoints.

The waypoints are composed by a vector where each element must contains:

a field named time which can be either a single value or an interval for which other values hold; And one or more of the followings:
a list named end_effector containing [x, y, z] coordinates for the delta end-effector;
a list named joint_stiffness containing the stiffness value for each of the three upper motors;
a list named gripper filled with the gripper reference position and its stiffness value; Whenever these are not present in the Parameter Server, it is assigned the last available value.
Warning
Please note that each of the above list must contain the exact number of required values. Once waypoint are retrieved, the Cartesian Planner is called to compute the relative joint trajectory (linear in the Cartesian space) for each qbmove device. This is done through an inverse kinematics processing.

Parameters

node_handle The Node Handle in which the waypoints are parsed, under .../waypoints (if present).

See also
cartesianLinearPlanner(), computeJointTrajectories(), fillMotorJointTrajectories(), parseVector(), xmlCast()

Definition at line 668 of file qb_delta_kinematic_controller.cpp.

void qb_chain_controllers::DeltaKinematicController::starting ( const ros::Time & time )

inlineoverridevirtual

This is called from within the realtime thread just before the first call to update.

Parameters

time	The current time.

Reimplemented from controller_interface::ControllerBase.

Definition at line 87 of file qb_delta_kinematic_controller.h.

void DeltaKinematicController::startWaypointTrajectory ( )

protected

Parse all the waypoints, filter the whole generated joint trajectory and send the commands to the devices through their relative Action server.

This can be helpful in a cyclic waypoint behavior to recompute the trajectory from the last reached position (which can be different from the initial one).

See also: filterMotorJointTrajectories(), move(), parseWaypointTrajectory()

Definition at line 753 of file qb_delta_kinematic_controller.cpp.

void qb_chain_controllers::DeltaKinematicController::stopping ( const ros::Time & time )

inlineoverridevirtual

This is called from within the realtime thread just after the last update call before the controller is stopped.

Parameters

time	The current time.

Reimplemented from controller_interface::ControllerBase.

Definition at line 93 of file qb_delta_kinematic_controller.h.

void DeltaKinematicController::targetPosesCallback ( const geometry_msgs::PointStamped & msg )

protected

Call the Cartesian Planner to compute the joint trajectory from the current position of the end-effector to the given target.

The trajectory is filled in a proper structure which is sent to the Action server of each qbmove.

Parameters

msg	The target end-effector position.

See also: cartesianLinearPlanner(), computeJointTrajectories(), move()

Definition at line 762 of file qb_delta_kinematic_controller.cpp.

void DeltaKinematicController::update	(	const ros::Time &	time,
		const ros::Duration &	period
	)

overridevirtual

Update the delta state.

This is called periodically by the realtime thread when the controller is running.

Parameters

time	The current time.
period	The time passed since the last call to update.

Implements controller_interface::ControllerBase.

Definition at line 773 of file qb_delta_kinematic_controller.cpp.

template<class T >

T DeltaKinematicController::xmlCast ( XmlRpc::XmlRpcValue xml_value )

protected

Cast an XmlRpcValue from TypeDouble, TypeInt or TypeBoolean to the specified template type.

This is necessary since XmlRpc does not handle conversion among basic types: it throws an exception if an improper cast is invoked though (e.g. from int to double).

Template Parameters

T	The type to cast to.

Parameters

xml_value The wrong-casted value.

Returns: The casted value.

See also: parseWaypointTrajectory(), parseVector()

Definition at line 787 of file qb_delta_kinematic_controller.cpp.

Member Data Documentation

ros::CallbackQueuePtr qb_chain_controllers::DeltaKinematicController::callback_queue_

protected

Definition at line 103 of file qb_delta_kinematic_controller.h.

visualization_msgs::InteractiveMarker qb_chain_controllers::DeltaKinematicController::controls_

protected

Definition at line 113 of file qb_delta_kinematic_controller.h.

geometry_msgs::Point qb_chain_controllers::DeltaKinematicController::ee_offset_

protected

Definition at line 122 of file qb_delta_kinematic_controller.h.

bool qb_chain_controllers::DeltaKinematicController::feedback_active_

protected

Definition at line 115 of file qb_delta_kinematic_controller.h.

geometry_msgs::Point qb_chain_controllers::DeltaKinematicController::feedback_position_old_

protected

Definition at line 114 of file qb_delta_kinematic_controller.h.

std::vector<double> qb_chain_controllers::DeltaKinematicController::filter_param_a_

protected

Definition at line 129 of file qb_delta_kinematic_controller.h.

std::vector<double> qb_chain_controllers::DeltaKinematicController::filter_param_b_

protected

Definition at line 130 of file qb_delta_kinematic_controller.h.

bool qb_chain_controllers::DeltaKinematicController::filter_trajectory_

protected

Definition at line 128 of file qb_delta_kinematic_controller.h.

ros::Publisher qb_chain_controllers::DeltaKinematicController::free_joint_state_publisher_

protected

Definition at line 107 of file qb_delta_kinematic_controller.h.

std::unique_ptr<interactive_markers::InteractiveMarkerServer> qb_chain_controllers::DeltaKinematicController::interactive_commands_server_

protected

Definition at line 112 of file qb_delta_kinematic_controller.h.

double qb_chain_controllers::DeltaKinematicController::max_displacement_

protected

Definition at line 127 of file qb_delta_kinematic_controller.h.

std::map<std::string, std::unique_ptr<actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> > > qb_chain_controllers::DeltaKinematicController::motor_action_clients_

protected

Definition at line 118 of file qb_delta_kinematic_controller.h.

std::map<std::string, std::vector<std::string> > qb_chain_controllers::DeltaKinematicController::motor_joint_names_

protected

Definition at line 120 of file qb_delta_kinematic_controller.h.

std::map<std::string, trajectory_msgs::JointTrajectory> qb_chain_controllers::DeltaKinematicController::motor_joint_trajectories_

protected

Definition at line 121 of file qb_delta_kinematic_controller.h.

std::map<std::string, hardware_interface::JointHandle> qb_chain_controllers::DeltaKinematicController::motor_joints_

protected

Definition at line 119 of file qb_delta_kinematic_controller.h.

std::vector<std::string> qb_chain_controllers::DeltaKinematicController::motor_names_

protected

Definition at line 117 of file qb_delta_kinematic_controller.h.

ros::NodeHandle qb_chain_controllers::DeltaKinematicController::node_handle_

protected

Definition at line 105 of file qb_delta_kinematic_controller.h.

ros::NodeHandle qb_chain_controllers::DeltaKinematicController::node_handle_control_

protected

Definition at line 106 of file qb_delta_kinematic_controller.h.

ros::AsyncSpinner qb_chain_controllers::DeltaKinematicController::spinner_

protected

Definition at line 104 of file qb_delta_kinematic_controller.h.

ros::Subscriber qb_chain_controllers::DeltaKinematicController::target_poses_sub_

protected

Definition at line 108 of file qb_delta_kinematic_controller.h.

tf2_ros::TransformBroadcaster qb_chain_controllers::DeltaKinematicController::tf_broadcaster_

protected

Definition at line 110 of file qb_delta_kinematic_controller.h.

bool qb_chain_controllers::DeltaKinematicController::use_interactive_markers_

protected

Definition at line 124 of file qb_delta_kinematic_controller.h.

bool qb_chain_controllers::DeltaKinematicController::use_waypoints_

protected

Definition at line 125 of file qb_delta_kinematic_controller.h.

std::string qb_chain_controllers::DeltaKinematicController::waypoint_namespace_

protected

Definition at line 126 of file qb_delta_kinematic_controller.h.

ros::WallTimer qb_chain_controllers::DeltaKinematicController::waypoint_setup_timer_

protected

Definition at line 109 of file qb_delta_kinematic_controller.h.

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

Public Member Functions

Protected Member Functions

Protected Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation