Allows a user to control the force or stiffness applied by joints. More...

#include <ee_cart_imped_control.hpp>

Inheritance diagram for ee_cart_imped_control_ns::EECartImpedControlClass:

Classes
class	EECartImpedData
	Class for storing a trajectory and associated information. More...
Public Member Functions
bool	init (pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
	Controller initialization in non-realtime.
void	starting ()
	Controller startup in realtime.
void	stopping ()
	Controller stopping in realtime.
void	update ()
	Controller update loop in realtime.
Private Types
typedef std::vector < ee_cart_imped_msgs::StiffPoint >	EECartImpedTraj
Private Member Functions
void	commandCB (const ee_cart_imped_msgs::EECartImpedGoalConstPtr &msg)
	Callback when a new goal is received.
double	linearlyInterpolate (double time, double startTime, double endTime, double startValue, double endValue)
	Linearly interpolates between trajectory points.
ee_cart_imped_msgs::StiffPoint	sampleInterpolation ()
	Samples the interpolation and returns the point the joints should currently be trying to achieve.
Private Attributes
pr2_mechanism_model::Chain	chain_
	The chain of links and joints in PR2 language for reference and commanding.
boost::scoped_ptr < realtime_tools::RealtimePublisher < ee_cart_imped_msgs::EECartImpedFeedback > >	controller_state_publisher_
	State publisher, published every 10 updates.
realtime_tools::RealtimeBox < boost::shared_ptr< const EECartImpedData > >	desired_poses_box_
	Desired positions.
KDL::Wrench	F_
	Cartesian effort.
KDL::Wrench	Fdes_
	Desired Cartesian force.
KDL::Jacobian	J_
	Jacobian.
boost::scoped_ptr < KDL::ChainJntToJacSolver >	jnt_to_jac_solver_
	KDL Solver performing the joint angles to Jacobian calculation.
boost::scoped_ptr < KDL::ChainFkSolverPos >	jnt_to_pose_solver_
	KDL Solver performing the joint angles to Cartesian pose calculation.
KDL::Twist	Kd_
	Derivative gains.
KDL::Chain	kdl_chain_
	The chain of links and joints in KDL language.
KDL::Twist	Kp_
	Proportional gains.
double	last_goal_starting_time_
	The time at which the goal we saw on the last iteration was started.
ee_cart_imped_msgs::StiffPoint	last_point_
	The actual position of the robot when we achieved the last point on the trajectory.
ros::Time	last_time_
	Time of the last servo cycle.
ros::NodeHandle	node_
KDL::JntArray	q_
	Joint positions.
KDL::JntArrayVel	qdot_
	Joint velocities.
pr2_mechanism_model::Chain	read_only_chain_
	The chain of links and joints in PR2 language for reference only.
pr2_mechanism_model::RobotState *	robot_state_
	The current robot state.
ros::Subscriber	subscriber_
KDL::JntArray	tau_
	Joint torques.
KDL::JntArray	tau_act_
int	updates_
	The number of updates to the joint position.
KDL::Frame	x_
	Tip pose.
KDL::Frame	xd_
	Tip desired pose.
KDL::Twist	xdot_
	Cartesian velocity.
KDL::Twist	xerr_
	Cartesian error.

Detailed Description

Allows a user to control the force or stiffness applied by joints.

This class contains the realtime controller for controlling joints by stiffness or force. To use this controller, you should use the action wrapper, EECartImpedAction.

Definition at line 41 of file ee_cart_imped_control.hpp.

Member Typedef Documentation

typedef std::vector<ee_cart_imped_msgs::StiffPoint> ee_cart_imped_control_ns::EECartImpedControlClass::EECartImpedTraj [private]

Definition at line 106 of file ee_cart_imped_control.hpp.

Member Function Documentation

void EECartImpedControlClass::commandCB ( const ee_cart_imped_msgs::EECartImpedGoalConstPtr & msg ) [private]

Callback when a new goal is received.

Cancels the currently active goal (if there is one), restarts the controller and asks it to execute the new goal.

Parameters:

msg	the new goal

Definition at line 169 of file ee_cart_imped_control.cpp.

bool EECartImpedControlClass::init	(	pr2_mechanism_model::RobotState *	robot,
		ros::NodeHandle &	n
	)		`[virtual]`

Controller initialization in non-realtime.

Initializes the controller on first startup. Prepares the kinematics, pre-allocates the variables, subscribes to command and advertises state.

Parameters:

robot	The current robot state
n	A node handle for subscribing and advertising topics

Returns:: True on successful initialization, false otherwise

Implements pr2_controller_interface::Controller.

Definition at line 212 of file ee_cart_imped_control.cpp.

double EECartImpedControlClass::linearlyInterpolate	(	double	time,
		double	startTime,
		double	endTime,
		double	startValue,
		double	endValue
	)		`[private]`

Linearly interpolates between trajectory points.

Linearly interpolates between startValue and endValue over the time period startTime to endTime of the current goal.

Parameters:

time	[seconds] that this goal has been running
startTime	time that the goal began running
endTime	time that the goal should finish running
startValue	the values at the beginning of the interpolation (i.e. the values at startTime)
endValue	the values at the end of the interpolation (i.e. the values at endTime)

Returns:: ((time - startTime) * (endValue - startValue) / (endTime - startTime)) + startValue;

Definition at line 13 of file ee_cart_imped_control.cpp.

ee_cart_imped_msgs::StiffPoint EECartImpedControlClass::sampleInterpolation ( ) [private]

Samples the interpolation and returns the point the joints should currently be trying to achieve.

Samples the interpolation, using EECartImpedControlClass::linearlyInterpolate, between the current trajectory point and the next trajectory point. Interpolation is done for position and orientation, but not for wrench or stiffness as generally it is desired that wrench or stiffness be constant over a specific trajectory segment.

Returns:: A StiffPoint that is the point the joints should attempt to achieve on this timestep.

Definition at line 24 of file ee_cart_imped_control.cpp.

void EECartImpedControlClass::starting ( ) [virtual]

Controller startup in realtime.

Resets the controller to prepare for a new goal. Sets the desired position and orientation to be the current position and orientation and sets the joints to have maximum stiffness so that they hold their current position.

Hold current position trajectory

Reimplemented from pr2_controller_interface::Controller.

Definition at line 291 of file ee_cart_imped_control.cpp.

void EECartImpedControlClass::stopping ( ) [virtual]

Controller stopping in realtime.

Calls EECartImpedControlClass::starting() to lock the joints into their current position.

Reimplemented from pr2_controller_interface::Controller.

Definition at line 502 of file ee_cart_imped_control.cpp.

void EECartImpedControlClass::update ( void ) [virtual]

Controller update loop in realtime.

A PD controller for achieving the trajectory. Uses EECartImpedControlClass::sampleInterpolation to find the point that should be achieved on the current timestep. Converts this point (which is in Cartesian coordinates) to joint angles and uses a PD update (in the case of stiffness) to send to the joints the correct force.

Implements pr2_controller_interface::Controller.

Definition at line 343 of file ee_cart_imped_control.cpp.