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 |
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.
| def articulate_cart_server::articulateCartServer::__init__ | ( | self, | ||
| use_tf_grippers, | ||||
| use_gripper_midpoint, | ||||
| tf_object_name | ||||
| ) |
| 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.
| 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.
Definition at line 185 of file articulate_cart_server.py.
Create a PoseStamped message with initial cart pose.
Definition at line 178 of file articulate_cart_server.py.
Set object poses from grippers if requested:
read object pose from parameter server
Definition at line 166 of file articulate_cart_server.py.
Definition at line 198 of file articulate_cart_server.py.
Definition at line 197 of file articulate_cart_server.py.
Definition at line 194 of file articulate_cart_server.py.
Load cart workspace constraints from parameter server.
Definition at line 193 of file articulate_cart_server.py.
Definition at line 196 of file articulate_cart_server.py.
Definition at line 195 of file articulate_cart_server.py.
Definition at line 139 of file articulate_cart_server.py.
Definition at line 98 of file articulate_cart_server.py.
subscriber for goal topic
Definition at line 111 of file articulate_cart_server.py.
Definition at line 131 of file articulate_cart_server.py.
Definition at line 259 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.
Definition at line 116 of file articulate_cart_server.py.
Definition at line 260 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.
Definition at line 129 of file articulate_cart_server.py.
Definition at line 118 of file articulate_cart_server.py.
Definition at line 189 of file articulate_cart_server.py.
Definition at line 151 of file articulate_cart_server.py.
Definition at line 122 of file articulate_cart_server.py.
Definition at line 190 of file articulate_cart_server.py.
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 95 of file articulate_cart_server.py.
Reimplemented in fake_articulate_cart_server::fakeArticulateCartServer.
Definition at line 320 of file articulate_cart_server.py.
Reload solver transforms if necessary.
Hard-code a set of arm postures
Definition at line 213 of file articulate_cart_server.py.
Definition at line 130 of file articulate_cart_server.py.
Definition at line 253 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.
publisher for arm cartesian pose/twist controller
Definition at line 115 of file articulate_cart_server.py.
Definition at line 254 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.
subscriber for arm state errors
Definition at line 128 of file articulate_cart_server.py.
Definition at line 117 of file articulate_cart_server.py.
Pre-compute relative pose of left gripper (for twist cb drift error).
Definition at line 188 of file articulate_cart_server.py.
Update initial effector poses if requested.
Read current gripper transforms (needed if reloading solver, or computing drift)
Definition at line 150 of file articulate_cart_server.py.
Publishers for teleop_controllers' posture topics.
Definition at line 121 of file articulate_cart_server.py.
get handles for manipulation_transforms service calls
Definition at line 137 of file articulate_cart_server.py.
Definition at line 74 of file articulate_cart_server.py.
Definition at line 244 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.
Attach to all topics and services.
Definition at line 80 of file articulate_cart_server.py.
Definition at line 81 of file articulate_cart_server.py.
Publisher for when when an invalid pose is received.
Definition at line 125 of file articulate_cart_server.py.
