takes either a goal pose or a trajectory and passes local goals to the MPC controller to try to make the controller follow it. More...

Public Member Functions
def	__init__
	Constructor.
def	angularDistanceBetweenPoses
	Return the angle between two quaternions (axis-angle representation)
def	distanceBetweenPoses
	Euclidian distance between two poses.
def	distanceBetweenPostures
	Max absolute distance between joints postures.
def	generateWaypoint
	Publishes the next waypoint for the control to try to achieve.
def	getMessageHeader
	Returns a message header (std_msgs object), populated with the time, frame_id and sequence number.
def	getOrientationStep
	Returns a linearly interpolated step towards the goal orientation from the current orientation (using SLERP)
def	getPositionStep
	Returns a linearly interpolated step towards the goal pos from the current pos.
def	getPostureFromTrajectory
	Pull postures from the trajectory deque.
def	getWaypointFromTrajectory
	Try to get a waypoint pose from the trajectory deque.
def	goalPoseCallback
	Update goal pose.
def	initCody
	Initialise 3DOF Sim kinematics.
def	initComms
	Initialise all publishers/subscribers used by the waypoint generator.
def	initCrona
def	initPR2
	Initialise PR2 kinematics.
def	initSim3
	Initialise 3DOF Sim kinematics.
def	jointTrajectoryCallback
	Store a joint angle trajectory in the deque.
def	poseTrajectoryCallback
	Store a trajectory of poses in the deque.
def	robotStateCallback
	Store the current pose from the haptic state publisher.
def	setControllerWeights
	Update the weights used by the MPC.
def	start
	Start the waypoint generator publishing waypoints.
def	straightLineTrajectory
	Returns the next waypoint along a straight line trajectory from the current gripper pose to the goal pose.
Public Attributes
	current_trajectory_msg
	goal_posture_pub
	joint_angles
	mode
	mpc_weights_pub
	pose_traj_viz_pub
	pose_waypoint_pub
	robot_kinematics
	tf_listener
Static Public Attributes
tuple	at_posture_threshold = numpy.radians(2.0)
tuple	at_waypoint_ang_threshold = numpy.radians(5.0)
float	at_waypoint_threshold = 0.02
string	base_path = "/haptic_mpc"
string	cody_param_path = "/cody"
string	crona_param_path = "/crona"
	current_gripper_waypoint = None
string	current_pose_topic = "/haptic_mpc/robot_state"
tuple	current_trajectory_deque = collections.deque()
string	default_eq_gen_type = 'mpc_qs_1'
	epcon = None
	eq_gen_type = None
tuple	goal_lock = threading.RLock()
	goal_pose = None
string	goal_pose_array_topic = "/haptic_mpc/goal_pose_array"
string	goal_pose_topic = "/haptic_mpc/goal_pose"
	gripper_pose = None
float	max_ang_step = 0.02
float	max_pos_step = 0.05
tuple	max_posture_step = numpy.radians(5.0)
string	mode = "none"
int	msg_seq = 0
	node_name = None
	opt = None
string	param_path = ""
string	pr2_param_path = "/pr2"
float	rate = 25.0
	robot = None
	robot_name = None
	scl = None
	sensor = None
string	sim_param_path = "/sim3"
string	simcody_param_path = "/simcody"
	skin_topic_list = None
tuple	state_lock = threading.RLock()
tuple	traj_lock = threading.RLock()
string	waypoint_pose_topic = "/haptic_mpc/traj_pose"

Detailed Description

takes either a goal pose or a trajectory and passes local goals to the MPC controller to try to make the controller follow it.

Definition at line 48 of file waypoint_generator.py.

Constructor & Destructor Documentation

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.__init__	(	self,
		node_name,
		opt
	)

Constructor.

Calls functions to initialise robot specific parameters, then initialises all publishers/subscribers.

Definition at line 103 of file waypoint_generator.py.

Member Function Documentation

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.angularDistanceBetweenPoses	(	self,
		poseA,
		poseB
	)

Return the angle between two quaternions (axis-angle representation)

Parameters:

poseA	geometry_msgs.msg.Pose
poseB	geometry_msgs.msg.Pose

Returns:: The angular distance between the two pose orientations.

Definition at line 403 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.distanceBetweenPoses	(	self,
		poseA,
		poseB
	)

Euclidian distance between two poses.

Ignores differences in orientation.

Parameters:

poseA	geometry_msgs.msg.Pose
poseB	geometry_msgs.msg.Pose

Returns:: The euclidian distance between the two pose positions.

Definition at line 393 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.distanceBetweenPostures	(	self,
		postureA,
		postureB
	)

Max absolute distance between joints postures.

Definition at line 379 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.generateWaypoint ( self )

Publishes the next waypoint for the control to try to achieve.

If we have a valid trajectory, get the next waypoint from that. If not and we have a valid end goal pose instead, move in a straight line towards it. If we have neither, don't publish anything.

Definition at line 470 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.getMessageHeader ( self )

Returns a message header (std_msgs object), populated with the time, frame_id and sequence number.

Definition at line 369 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.getOrientationStep	(	self,
		q_h_orient,
		q_g_orient,
		max_ang_step
	)

Returns a linearly interpolated step towards the goal orientation from the current orientation (using SLERP)

Parameters:

q_h_orient	Current hand orientation as a quaternion in numpy array form (assumed to be [x,y,z,w])
q_g_orient	Current goal orientation as a quaternion in numpy array form (assumed to be [x,y,z,w])
max_ang_step	A scalar max step size for orientation (in radians).

Returns:: An interpolated step in orientation towards the goal.

Definition at line 356 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.getPositionStep	(	self,
		current_pos,
		goal_pos,
		max_pos_step
	)

Returns a linearly interpolated step towards the goal pos from the current pos.

Parameters:

current_pos	Current position as a numpy array (assumed to be [x,y,z])
goal_pos	Goal position as a numpy array (assumed to be [x,y,z])
max_pos_step	A scalar max step size for position (in metres).

Returns:: An interpolated step in position towards the goal.

Definition at line 339 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.getPostureFromTrajectory ( self )

Pull postures from the trajectory deque.

Definition at line 439 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.getWaypointFromTrajectory ( self )

Try to get a waypoint pose from the trajectory deque.

Also trims the trajectory to reduce the number of waypoint. Scans through the trajectory to find the next pose that is at least max_pos_step away (or the last pose in the deque).

Returns:: A Pose object if there is a currently stored trajectory, otherwise None.

Definition at line 414 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.goalPoseCallback	(	self,
		msg
	)

Update goal pose.

Parameters:

msg	A geometry_msgs.msg.PoseStamped object.

Definition at line 191 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.initCody ( self )

Initialise 3DOF Sim kinematics.

NB: This doesn't actually do anything and is added mostly for structural consistency.

Definition at line 129 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.initComms ( self )

Initialise all publishers/subscribers used by the waypoint generator.

Definition at line 172 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.initCrona ( self )

Definition at line 155 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.initPR2 ( self )

Initialise PR2 kinematics.

NB: Only used for joint limits, will eventually be removed once these are passed with the robot state.

Definition at line 135 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.initSim3 ( self )

Initialise 3DOF Sim kinematics.

NB: This doesn't actually do anything and is added mostly for structural consistency.

Definition at line 151 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.jointTrajectoryCallback	(	self,
		msg
	)

Store a joint angle trajectory in the deque.

Performs forward kinematics to convert it to end effector poses in the torso frame.

Parameters:

msg	A trajectory_msgs.msg.JointTrajectory object.

Definition at line 262 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.poseTrajectoryCallback	(	self,
		msg
	)

Store a trajectory of poses in the deque.

Converts it to the 'torso_frame' if required.

Parameters:

msg	A geometry_msgs.msg.PoseArray object

Definition at line 220 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.robotStateCallback	(	self,
		msg
	)

Store the current pose from the haptic state publisher.

Parameters:

msg	RobotHapticState messge object

Definition at line 213 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.setControllerWeights	(	self,
		position_weight,
		orient_weight,
		posture_weight
	)

Update the weights used by the MPC.

Definition at line 292 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.start ( self )

Start the waypoint generator publishing waypoints.

Definition at line 528 of file waypoint_generator.py.

def hrl_haptic_mpc.waypoint_generator.WaypointGenerator.straightLineTrajectory	(	self,
		current_pose,
		goal_pose,
		max_pos_step,
		max_ang_step
	)

Returns the next waypoint along a straight line trajectory from the current gripper pose to the goal pose.

The step size towards the goal is configurable through the parameters passed in.

Parameters:

current_pose	geometry_msgs.msg.Pose
goal_pose	geometry_msgs.msg.Pose
max_pos_step	= scalar float for position step size to take (metres)
max_ang_step	= scalar float for orientation step size to take (radians)