Haptic Model Predictive Controller class. More...

Public Member Functions
def	__init__
	Constructor.
def	contactForceTransformationMatrix
	Calculates the force transformation matrix from a list of force normals.
def	contactStiffnessMatrix
	Computes the contact stiffness matrix, K_ci.
def	deltaForceBounds
	Compute bounds for delta_f.
def	deltaQpJepGen
	Main control calculation.
def	enableHapticMPC
	Enable Haptic MPC service handler (default is enabled).
def	getDesiredJointAngles
	getDesiredJointAngles accessor function.
def	getJointAngles
	getJointAngles accessor function.
def	getJointStiffness
	getJointStiffness accessor function.
def	getMPCData
	getMPCData accessor function.
def	getSkinData
	getSkinData accessor function.
def	goalDeltaPosture
	Process the goal posture input before sending it to the controller.
def	goalMotionForHand
	Interpolate a step towards the given goal position.
def	goalOrientationInQuat
	Interpolate a step towards the given goal orientation.
def	goalPoseCallback
	Store the current trajectory goal.
def	goalPostureCallback
	Store the current posture goal.
def	initComms
	Initialise the ROS communications - init node, subscribe to the robot state and goal pos, publish JEPs.
def	initControlParametersFromServer
	Read parameters from the ROS parameter server and store them.
def	initMPCData
	Builds the MPC data structure from the individual parameters This is just a convenience data structure to amalgamate the data.
def	initRobot
	Initialise the robot joint limits.
def	mpcMonitorCallback
	Store the state information from the monitor node.
def	publishDeltaControlValues
	Publishes a list of Deltas for the desired joint positions.
def	publishDesiredJointAngles
	Publish a desired joint position list directly (rather than deltas)
def	robotStateCallback
	Store the robot haptic state.
def	signal_handler
	Handler for Ctrl-C signals.
def	start
	Start the control loop once the controller is initialised.
def	updateController
	Main control function.
def	updateWeightsCallback
	Update the position/orientation weights used by the controller.
Public Attributes
	allowable_contact_force
	currently_in_deadzone
	deadzone_angle
	deadzone_distance
	delta_q_des_pub
	desired_joint_angles
	enable_mpc_srv
	end_effector_orient_quat
	end_effector_pos
	estimate_contact_stiffness
	force_reduction_goal
	force_weight
	frequency
	gain_lock
	Monitor state lock.
	goal_lock
	Haptic state lock.
	goal_orient_quat
	goal_pos
	goal_pose_sub
	goal_posture
	goal_posture_sub
	goal_velocity_for_hand
	Jc
	Je
	jerk_opt_weight
	joint_angles
	joint_damping
	joint_limits_max
	joint_limits_min
	joint_names
	Current MPC errors.
	joint_stiffness
	joint_velocities
	last_msg_time
	Haptic state message.
	ma_to_m
	max_delta_force_mag
	max_theta_step
	monitor_lock
	Goal state lock.
	mpc_data
	Goal posture lock.
	mpc_enabled
	mpc_error
	Current MPC state.
	mpc_monitor_sub
	mpc_state
	mpc_weights_pub
	mpc_weights_sub
	msg
	MPCData data structure.
	opt
	orient_step
	orient_weight
	pos_weight
	position_step
	posture_lock
	Controller gain state lock.
	posture_step
	posture_weight
	q_des_pub
	robot_state_sub
	skin_data
	state_lock
	static_contact_stiffness_estimate
	stopping_force
	theta_step_scale
	time_step
	timeout
	waiting_for_no_errors
	waiting_to_resume

Detailed Description

Haptic Model Predictive Controller class.

Definition at line 132 of file haptic_mpc.py.

Constructor & Destructor Documentation

def hrl_haptic_mpc.haptic_mpc.HapticMPC.__init__	(	self,
		opt,
		node_name = `"haptic_mpc"`
	)

Constructor.

Parameters:

opt	optparse options. Should be created using the helper utility script for reliability.
node_name	Name used for the ROS node.

Definition at line 136 of file haptic_mpc.py.

Member Function Documentation

def hrl_haptic_mpc.haptic_mpc.HapticMPC.contactForceTransformationMatrix	(	self,
		n_l
	)

Calculates the force transformation matrix from a list of force normals.

Parameters:

n_l	List of force normals

Returns:: List of transformation matrices (rotations)

Definition at line 782 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.contactStiffnessMatrix	(	self,
		n_l,
		k_default,
		k_est_min = `100.0`,
		k_est_max = `100000.0`
	)

Computes the contact stiffness matrix, K_ci.

Parameters:

n_l	List of normal forces param k_default Default stiffness, N/m. Default is typically 200.0-1000.0

Definition at line 745 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.deltaForceBounds	(	self,
		f_n,
		max_pushing_force,
		max_pulling_force,
		max_pushing_force_increase,
		max_pushing_force_decrease,
		min_decrease_when_over_max_force,
		max_decrease_when_over_max_force
	)

Compute bounds for delta_f.

Return values:

delta_f_min	Minimum bound
delta_f_max	Maximum bound

Definition at line 812 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.deltaQpJepGen	(	self,
		mpc_dat
	)

Main control calculation.

Parameters:

mpc_dat An MPCData class, populated with relevant control parameters and current data

Returns:: A list of joint equilibrium point deltas, ie, a desired increment to the current joint position(s).

Definition at line 572 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.enableHapticMPC ( req )

Enable Haptic MPC service handler (default is enabled).

Definition at line 953 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.getDesiredJointAngles ( self )

getDesiredJointAngles accessor function.

Returns:: A copy of the desired joint angles list, ie, the current arm controller setpoint.

Definition at line 283 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.getJointAngles ( self )

getJointAngles accessor function.

Returns:: A copy of the buffered joint angles list.

Definition at line 276 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.getJointStiffness ( self )

getJointStiffness accessor function.

Returns:: A copy of the joint stiffness parameters used by the controller.

Definition at line 290 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.getMPCData ( self )

getMPCData accessor function.

Returns a copy of the control data structure.

Definition at line 297 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.getSkinData ( self )

getSkinData accessor function.

Returns:: A copy of the skin_data dictionary, containing the latest taxel data.

Definition at line 269 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.goalDeltaPosture	(	self,
		goal_posture,
		current_posture,
		max_theta_step,
		num_steps_scale
	)

Process the goal posture input before sending it to the controller.

Parameters:

goal_posture	List of desired joint angles. Should be
current_posture	List of current joint angles

Definition at line 544 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.goalMotionForHand	(	self,
		x_h,
		x_g,
		dist_g
	)

Interpolate a step towards the given goal position.

Returns:: A goal delta in position as a numpy matrix.

Parameters:

x_h	The current hand position as a numpy matrix: [x,y,z]
x_g	The current goal position as a numpy matrix: [x,y,z]

Definition at line 520 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.goalOrientationInQuat	(	self,
		q_h_orient,
		q_g_orient,
		max_ang_step
	)

Interpolate a step towards the given goal orientation.

Parameters:

q_h_orient	The current hand orientation as a quaternion in list form: [x,y,z,w]
q_g_orient	The current goal orientation as a quaternion in list form: [x,y,z,w]

Returns:: A desired change in orientation as a delta: TODO: clean this up so it doesn't use the "step" multiplier

Definition at line 499 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.goalPoseCallback	(	self,
		msg
	)

Store the current trajectory goal.

The controller will always attempt a linear path to this.

Parameters:

msg	A PoseStamped message

Definition at line 452 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.goalPostureCallback	(	self,
		msg
	)

Store the current posture goal.

The controller attempts to achieve this posture. TODO: Define message type. Ideally just joint angles. Float64 list?

Parameters:

msg	An array of Float64s. Type hrl_msgs.FloatArray.

Definition at line 460 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.initComms	(	self,
		node_name
	)

Initialise the ROS communications - init node, subscribe to the robot state and goal pos, publish JEPs.

Definition at line 936 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.initControlParametersFromServer ( self )

Read parameters from the ROS parameter server and store them.

Definition at line 196 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.initMPCData ( self )

Builds the MPC data structure from the individual parameters This is just a convenience data structure to amalgamate the data.

Also does data validation.

Definition at line 304 of file haptic_mpc.py.

def hrl_haptic_mpc.haptic_mpc.HapticMPC.initRobot ( self )

Initialise the robot joint limits.

This relies on something (usually the haptic state node) pushing the appropriate joint limits to the param server before execution. NB: All joint limits are specified in degrees and converted to radians here (easier to understand)

Definition at line 233 of file haptic_mpc.py.