articulate_cart_server.articulateCartServer Class Reference

Inheritance diagram for articulate_cart_server.articulateCartServer:

List of all members.

Public Member Functions
def	__init__
def	euler_list_to_pose_msg
def	get_l_drift_twist
def	get_rel_pose
def	get_tf_transform
def	get_transform_param
def	get_twist_SSE
def	interpolate_pose_tup
def	interpolate_twist_tup
def	pose_msg_to_euler_list
def	pose_tup_to_matrix
def	pose_tup_to_msg
def	ready_pose_cb
def	set_new_pose
def	set_transform_param
def	twist_msg_to_tup
def	twist_tup_to_msg
Public Attributes
	goal_received
	Attach to all topics and services.
	goal_sub
	invalid_pose_pub
	Publisher for when when an invalid pose is received.
Static Public Attributes
float	errthresh = 0.02
Private Member Functions
def	_check_cart_pose
def	_fake_twist_controller
def	_fake_twist_goal_cb
def	_initialize_poses
def	_initialize_ros_coms
def	_l_arm_err_cb
def	_pose_command_action
def	_posture_loop
def	_r_arm_err_cb
Private Attributes
	_cart_pose_euler
	_cart_pose_msg
	Create a PoseStamped message with initial cart pose.
	_cart_pose_tup
	Set object poses from grippers if requested:
	_cart_t_max
	_cart_t_min
	_cart_x_max
	_cart_x_min
	Load cart workspace constraints from parameter server.
	_cart_y_max
	_cart_y_min
	_cartPoseToGrippers_srv
	_fake_twist_loop_started
	_goal_sub
	subscriber for goal topic
	_l_arm_err_lock
	_l_arm_err_twist
	_l_arm_pose_pub
	_l_arm_SSE
	_l_arm_sub
	_l_arm_twist_pub
	_l_pose_mtr
	_l_pose_tup
	_l_posture_pub
	_l_relpose
	_pose_mutex
	Initialize object and gripper poses according to constructor params.
	_pose_resp
	_postures
	Reload solver transforms if necessary.
	_r_arm_err_lock
	_r_arm_err_twist
	_r_arm_pose_pub
	publisher for arm cartesian pose/twist controller
	_r_arm_SSE
	_r_arm_sub
	subscriber for arm state errors
	_r_arm_twist_pub
	_r_pose_mtr
	Pre-compute relative pose of left gripper (for twist cb drift error)
	_r_pose_tup
	Update initial effector poses if requested.
	_r_posture_pub
	Publishers for teleop_controllers' posture topics.
	_solver_reloadTransforms_srv
	get handles for manipulation_transforms service calls
	_tl
	_twist_req
Static Private Attributes
tuple	_l_arm_err_twist = Twist()
int	_l_arm_SSE = 0
tuple	_r_arm_err_twist = Twist()
int	_r_arm_SSE = 0

---

Detailed Description

A server for manipulating a cart being grasped by the grippers
of the pr2.  Uses manipulation_transforms to compute gripper poses satisfying
incoming goal requests.  Goals for the cart pose are specified in the
base_footprint frame.  manipulation_transforms is used to map this goal to
gripper poses in the base_footprint frame, which are then written
to the goal topics for teleop_controllers (make sure to specify base_footprint
as the frame_id rather than torso_lift_link).
Accepts goals as PoseStamped messages, and issues PoseStamped messages
to the arms.

Definition at line 48 of file articulate_cart_server.py.

---

Constructor & Destructor Documentation

def articulate_cart_server.articulateCartServer.__init__	(		self,
			use_tf_grippers,
			use_gripper_midpoint,
			tf_object_name
	)

Parameters:

use_tf_grippers,:	lookup gripper poses from tf and re-initialize manipulation_transforms
use_gripper_midpoint,:	set object initial pose to midpoint between grippers, and re-initialize manipulation_transforms
tf_object_name,:	if non-empty, attempt to lookup object initial pose on tf with given frame name, and re-initialize manipulation_transforms

Reimplemented in fake_articulate_cart_server.fakeArticulateCartServer.

Definition at line 72 of file articulate_cart_server.py.

---

Member Function Documentation

def articulate_cart_server.articulateCartServer._check_cart_pose	(		self,
			pose
	)		[private]

Returns true if pose is reachable given workspace constraints of cart

Definition at line 468 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer._fake_twist_controller	(		self,
			r = 20,
			recovery_horizon = 5
	)		[private]

Simulates the twist applied to the object, and updates the gripper poses accordingly.

Definition at line 270 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer._fake_twist_goal_cb	(		self,
			msg
	)		[private]

An alternative callback for twist messages that spawns an internal controller to simulate
the object twist action using pose commands

Definition at line 231 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer._initialize_poses	(		self,
			use_tf_grippers,
			use_gripper_midpoint,
			tf_object_name
	)		[private]

Definition at line 145 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer._initialize_ros_coms

(

self

)

[private]

Definition at line 109 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer._l_arm_err_cb	(		self,
			twist
	)		[private]

Definition at line 257 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer._pose_command_action

(

self

)

[private]

Send the target effector poses to the arm control topics
Assumes that self._pose_resp is up-to-date

Reimplemented in fake_articulate_cart_server.fakeArticulateCartServer.

Definition at line 323 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer._posture_loop	(		self,
			r_posture,
			l_posture
	)		[private]

Send updates to the posture topic every ROS second 

Definition at line 263 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer._r_arm_err_cb	(		self,
			twist
	)		[private]

Definition at line 251 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.euler_list_to_pose_msg	(		self,
			euler_pose
	)

convert to pose represented as a list [[vector],[euler angles]] to a Pose msg

Definition at line 390 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.get_l_drift_twist

(

self

)

Uses pose call to solver to compute error at left gripper, and returns as twist

Definition at line 433 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.get_rel_pose	(		self,
			p1,
			p2
	)

Compute relative pose of p2 in frame of p1 (4x4 numpy arrays)

Definition at line 377 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.get_tf_transform	(		self,
			ref_frame,
			target_frame
	)

An exception-handling method for querying a transform from tf

Definition at line 424 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.get_transform_param	(		self,
			name
	)

Returns the named transform as a pose tuple

Arguments:
- `name`: string matching a pose on the param server

Definition at line 337 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.get_twist_SSE	(		self,
			twist
	)

Definition at line 460 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.interpolate_pose_tup	(		self,
			p1,
			p2,
			blend = 0.5
	)

Returns weighted interpolation of two pose tuples ((vector), (quaternion))

Definition at line 412 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.interpolate_twist_tup	(		self,
			t1,
			t2,
			blend = 0.5
	)

Returns a weighted average of two twist tuples ((linear), (angular))

Definition at line 418 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.pose_msg_to_euler_list	(		self,
			Pose
	)

Returns a list representation of Pose [[vector],[euler angles]]

Definition at line 381 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.pose_tup_to_matrix	(		self,
			tup
	)

Returns a numpy 4x4 transform array from a pose provided in a tuple ((vector), (quaternion))

Definition at line 357 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.pose_tup_to_msg	(		self,
			tup
	)

Returns Pose message assembled from tf query tuple 

Definition at line 365 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.ready_pose_cb	(		self,
			msg
	)

Definition at line 106 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.set_new_pose	(		self,
			p
	)

Definition at line 317 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.set_transform_param	(		self,
			name,
			pose_tup
	)

Sets the provided transform param tuple on the paramater server in "cart format"

Arguments:
- `pose_tup`: a tuple containing ((trans),(rot))
- `name`: a string to set

Definition at line 348 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.twist_msg_to_tup	(		self,
			msg
	)

Returns a twist tuple ((linear), (angular)) from parameters in a Twist message 

Definition at line 396 of file articulate_cart_server.py.

def articulate_cart_server.articulateCartServer.twist_tup_to_msg	(		self,
			tup
	)

Returns a Twist message from parameters in a tuple ((linear), (angular))

Definition at line 401 of file articulate_cart_server.py.

---

Member Data Documentation

articulate_cart_server.articulateCartServer::_cart_pose_euler [private]

Definition at line 150 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_cart_pose_msg [private]

Create a PoseStamped message with initial cart pose.

Definition at line 150 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_cart_pose_tup [private]

Set object poses from grippers if requested:

read object pose from parameter server

Definition at line 148 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_cart_t_max [private]

Definition at line 152 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_cart_t_min [private]

Definition at line 152 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_cart_x_max [private]

Definition at line 152 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_cart_x_min [private]

Load cart workspace constraints from parameter server.

Definition at line 152 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_cart_y_max [private]

Definition at line 152 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_cart_y_min [private]

Definition at line 152 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_cartPoseToGrippers_srv [private]

Definition at line 115 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_fake_twist_loop_started [private]

Definition at line 75 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_goal_sub [private]

subscriber for goal topic

Definition at line 110 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_l_arm_err_lock [private]

Definition at line 114 of file articulate_cart_server.py.

tuple articulate_cart_server.articulateCartServer::_l_arm_err_twist = Twist() [static, private]

Definition at line 64 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_l_arm_err_twist [private]

Definition at line 257 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_l_arm_pose_pub [private]

Definition at line 111 of file articulate_cart_server.py.

int articulate_cart_server.articulateCartServer::_l_arm_SSE = 0 [static, private]

Definition at line 62 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_l_arm_SSE [private]

Definition at line 257 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_l_arm_sub [private]

Definition at line 114 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_l_arm_twist_pub [private]

Definition at line 111 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_l_pose_mtr [private]

Definition at line 151 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_l_pose_tup [private]

Definition at line 147 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_l_posture_pub [private]

Definition at line 112 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_l_relpose [private]

Definition at line 151 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_pose_mutex [private]

Initialize object and gripper poses according to constructor params.

Mutex for pose-related members {_cart_pose_tup, _cart_pose_msg, _pose_resp, _twist_req, _cart_pose_euler}

Definition at line 75 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_pose_resp [private]

Reimplemented in fake_articulate_cart_server.fakeArticulateCartServer.

Definition at line 317 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_postures [private]

Reload solver transforms if necessary.

Hard-code a set of arm postures

Definition at line 154 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_r_arm_err_lock [private]

Definition at line 114 of file articulate_cart_server.py.

tuple articulate_cart_server.articulateCartServer::_r_arm_err_twist = Twist() [static, private]

Definition at line 63 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_r_arm_err_twist [private]

Definition at line 251 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_r_arm_pose_pub [private]

publisher for arm cartesian pose/twist controller

Definition at line 111 of file articulate_cart_server.py.

int articulate_cart_server.articulateCartServer::_r_arm_SSE = 0 [static, private]

Definition at line 61 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_r_arm_SSE [private]

Definition at line 251 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_r_arm_sub [private]

subscriber for arm state errors

Definition at line 114 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_r_arm_twist_pub [private]

Definition at line 111 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_r_pose_mtr [private]

Pre-compute relative pose of left gripper (for twist cb drift error)

Definition at line 151 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_r_pose_tup [private]

Update initial effector poses if requested.

Read current gripper transforms (needed if reloading solver, or computing drift)

Definition at line 147 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_r_posture_pub [private]

Publishers for teleop_controllers' posture topics.

Definition at line 112 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_solver_reloadTransforms_srv [private]

get handles for manipulation_transforms service calls

Definition at line 115 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_tl [private]

Definition at line 72 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::_twist_req [private]

Definition at line 232 of file articulate_cart_server.py.

float articulate_cart_server.articulateCartServer::errthresh = 0.02 [static]

Definition at line 65 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::goal_received

Attach to all topics and services.

Definition at line 73 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::goal_sub

Definition at line 73 of file articulate_cart_server.py.

articulate_cart_server.articulateCartServer::invalid_pose_pub

Publisher for when when an invalid pose is received.

Definition at line 113 of file articulate_cart_server.py.

---

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

• articulate_cart_server.py