class to start/stop/switch between joint and Cartesian controllers More...
Public Member Functions | |
| def | __init__ |
| def | check_cartesian_done |
| check if the Cartesian controller thinks it's gotten to its goal (it's pretty loose about goals) | |
| def | check_cartesian_near_pose |
| check if we're near to a desired Cartesian pose (either PoseStamped or 7-list of position and quaternion in 'base_link' frame) by pos_thres(m) and rot_thres (rad) | |
| def | check_cartesian_really_done |
| check if both the Cartesian controllers think we're done and if we're close to where we want to be | |
| def | check_controller_states |
| figure out which controllers are loaded/started (fills in self.joint_controller_state, self.cartesian_controllers_state, and self.gripper_controller_state with 'not loaded', 'stopped', or 'running' | |
| def | check_controllers_ok |
| make sure the appropriate controllers (joint or Cartesian) are running/stopped mode is 'joint' or 'cartesian' | |
| def | check_joint_trajectory_done |
| check the state of the joint trajectory action (returns 1 if done, 0 otherwise) | |
| def | command_cartesian |
| send a single desired pose to the Cartesian controller (pose is either a PoseStamped or a 7-list of position and quaternion orientation; if the latter, put the frame in frame_id) | |
| def | command_gripper |
| send a command to the gripper action | |
| def | command_interpolated_ik |
| find and execute a joint-angle trajectory to perform a collision-free Cartesian movement if start_pose is None, starts from the current pose (if not None, will go straight to the start pose using the joint trajectory action) | |
| def | command_joint |
| send a single joint configuration to the joint trajectory action (takes in a 7-list of angles, nonblocking) | |
| def | command_joint_trajectory |
| send an entire joint trajectory to the joint trajectory action | |
| def | create_move_arm_goal |
| create a basic goal message for move_arm | |
| def | find_gripper_contact |
| tell the gripper to close until contact (on one or both finger pads) contacts_desired is "both", "left", "right", or "either" | |
| def | freeze_arm |
| tell the joint trajectory action to stop the arm where it is now | |
| def | get_current_arm_angles |
| use the joint_states_listener to get the arm angles | |
| def | get_current_gripper_opening |
| use the joint_states_listener to get the current gripper opening | |
| def | get_current_wrist_pose_stamped |
| return the current pose of the wrist as a PoseStamped | |
| def | get_find_gripper_contact_result |
| get the final result from find_gripper_contact | |
| def | get_find_gripper_contact_state |
| get the state from find_gripper_contact | |
| def | get_gripper_event_detector_state |
| get the state from the gripper event detector | |
| def | load_cartesian_controllers |
| load the Cartesian controllers with the current set of params on the param server | |
| def | load_joint_controllers |
| load the joint controller with the current set of params on the param server | |
| def | modify_move_arm_goal |
| if move_arm_joint returns an error code of START_STATE_IN_COLLISION, we can try to add allowed contact regions to get it out of the start collisions (appears to already be done in C++) | |
| def | move_arm_constrained |
| move the wrist to the desired location while keeping the current wrist orientation upright start angles are the joint angle start point for IK searches default start angles and location are for moving the arm to the side | |
| def | move_arm_joint |
| use move_arm to get to a desired joint configuration in a collision-free way | |
| def | move_arm_pose |
| use move_arm to get to a desired pose in a collision-free way (really, run IK and then call move arm to move to the IK joint angles) | |
| def | move_cartesian |
| move in Cartesian space using the Cartesian controllers if collision_aware, checks whether the path could be collision-free, but uses the Cartesian controllers to execute it if blocking, waits for completion, otherwise returns after sending the goal step_size only used if collision aware (size of steps to check for collisions) pos_thres and rot_thres are thresholds within which the goal is declared reached times out and quits after timeout, and waits settling_time after the controllers declare that they're done (which is usually way too early) | |
| def | move_cartesian_ik |
| move in Cartesian space using IK Cartesian interpolation if collision_aware, quits if the path is not collision-free if blocking, waits for completion, otherwise returns after sending the goal step_size is the step size for interpolation pos_thres and rot_thres are thresholds within which the goal is declared reached times out and quits after timeout, and waits settling_time after the controllers declare that they're done | |
| def | normalize_angle |
| normalize an angle for a continuous joint so that it's the closest version of the angle to the current angle (not +-2*pi) | |
| def | normalize_trajectory |
| normalize a trajectory (list of lists of joint angles), so that the desired angles are the nearest ones for the continuous joints (5 and 7) | |
| def | pplist |
| pretty-print list to string | |
| def | ppmat |
| pretty-print numpy matrix to string | |
| def | print_cartesian_pose |
| print the current Cartesian pose of the gripper | |
| def | reload_cartesian_controllers |
| re-load the Cartesian controllers with new parameters (change cart_params before running) | |
| def | reload_joint_controllers |
| re-load the joint controller with gains a fraction of the defaults | |
| def | return_cartesian_pose |
| return the current Cartesian pose of the gripper | |
| def | send_move_arm_goal |
| send a goal to move arm and wait for the result, modifying the goal if the start state is in collision | |
| def | start_cartesian_controllers |
| start the Cartesian controllers (need to be loaded already) | |
| def | start_gripper_controller |
| start the gripper controller (needs to be loaded already) | |
| def | start_gripper_event_detector |
| start up gripper event detector to detect when an object hits the table or when someone is trying to take an object from the robot | |
| def | start_gripper_grab |
| opens and closes the gripper to determine how hard to grasp the object, then starts up slip servo mode | |
| def | start_joint_controllers |
| just start the joint controllers (need to be loaded already) | |
| def | stop_all_controllers |
| stop all controllers | |
| def | stop_controllers |
| stop controllers that are currently running | |
| def | switch_to_cartesian_mode |
| stops the joint controllers, starts the Cartesian ones (both need to be loaded already) | |
| def | switch_to_joint_mode |
| stops the Cartesian controllers, starts the joint ones (both need to be loaded already) | |
| def | unload_cartesian_controllers |
| unload the Cartesian controllers (to re-load with new params) | |
| def | unload_joint_controllers |
| unload the joint controller | |
| def | wait_cartesian_really_done |
| wait for either the Cartesian pose to get near enough to a goal pose or timeout (a ROS duration) is reached or settling_time (a ROS duration) after the Cartesian controllers report that they're done | |
| def | wait_for_action_server |
| wait for an action server to be ready | |
| def | wait_for_service |
| wait for a service to be ready | |
| def | wait_joint_trajectory_done |
| wait for the joint trajectory action to finish (returns 1 if succeeded, 0 otherwise) timeout is a ROS duration; default (0) is infinite timeout returns 0 if timed out, 1 if succeeded | |
Public Attributes | |
| cart_params | |
| cartesian_cmd_service | |
| cartesian_controllers | |
| cartesian_controllers_state | |
| cartesian_moving_service | |
| cartesian_preempt_service | |
| gripper_action_client | |
| gripper_controller | |
| gripper_controller_state | |
| gripper_event_detector_action_client | |
| gripper_find_contact_action_client | |
| gripper_grab_action_client | |
| ik_utilities | |
| joint_action_client | |
| joint_controller | |
| joint_controller_state | |
| joint_names | |
| joint_params | |
| joint_states_listener | |
| list_controllers_service | |
| load_controller_service | |
| move_arm_client | |
| switch_controller_service | |
| tf_listener | |
| unload_controller_service | |
| using_slip_controller | |
| using_slip_detection | |
| whicharm | |
class to start/stop/switch between joint and Cartesian controllers
Definition at line 177 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::__init__ | ( | self, | ||
| whicharm, | ||||
tf_listener = None, |
||||
using_slip_controller = 0, |
||||
using_slip_detection = 0 | ||||
| ) |
Definition at line 180 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::check_cartesian_done | ( | self | ) |
check if the Cartesian controller thinks it's gotten to its goal (it's pretty loose about goals)
Definition at line 740 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::check_cartesian_near_pose | ( | self, | ||
| goal_pose, | ||||
| pos_thres, | ||||
| rot_thres | ||||
| ) |
check if we're near to a desired Cartesian pose (either PoseStamped or 7-list of position and quaternion in 'base_link' frame) by pos_thres(m) and rot_thres (rad)
Definition at line 747 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::check_cartesian_really_done | ( | self, | ||
| goal_pose, | ||||
| pos_thres, | ||||
| rot_thres | ||||
| ) |
check if both the Cartesian controllers think we're done and if we're close to where we want to be
Definition at line 771 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::check_controller_states | ( | self | ) |
figure out which controllers are loaded/started (fills in self.joint_controller_state, self.cartesian_controllers_state, and self.gripper_controller_state with 'not loaded', 'stopped', or 'running'
Definition at line 1014 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::check_controllers_ok | ( | self, | ||
| mode | ||||
| ) |
make sure the appropriate controllers (joint or Cartesian) are running/stopped mode is 'joint' or 'cartesian'
Definition at line 863 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::check_joint_trajectory_done | ( | self | ) |
check the state of the joint trajectory action (returns 1 if done, 0 otherwise)
Definition at line 968 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::command_cartesian | ( | self, | ||
| pose, | ||||
frame_id = 'base_link' | ||||
| ) |
send a single desired pose to the Cartesian controller (pose is either a PoseStamped or a 7-list of position and quaternion orientation; if the latter, put the frame in frame_id)
Definition at line 719 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::command_gripper | ( | self, | ||
| position, | ||||
| max_effort, | ||||
blocking = 0 | ||||
| ) |
send a command to the gripper action
Definition at line 993 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::command_interpolated_ik | ( | self, | ||
| end_pose, | ||||
start_pose = None, |
||||
collision_aware = 1, |
||||
step_size = .02, |
||||
max_joint_vel = .05 | ||||
| ) |
find and execute a joint-angle trajectory to perform a collision-free Cartesian movement if start_pose is None, starts from the current pose (if not None, will go straight to the start pose using the joint trajectory action)
Definition at line 658 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::command_joint | ( | self, | ||
| jointangles | ||||
| ) |
send a single joint configuration to the joint trajectory action (takes in a 7-list of angles, nonblocking)
Definition at line 903 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::command_joint_trajectory | ( | self, | ||
| trajectory, | ||||
max_joint_vel = 0.2, |
||||
current_angles = None, |
||||
blocking = 0, |
||||
times_and_vels = None | ||||
| ) |
send an entire joint trajectory to the joint trajectory action
Definition at line 918 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::create_move_arm_goal | ( | self | ) |
create a basic goal message for move_arm
Definition at line 435 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::find_gripper_contact | ( | self, | ||
| contacts_desired, | ||||
zero_fingertips = 0, |
||||
blocking = 1, |
||||
timeout = 12. | ||||
| ) |
tell the gripper to close until contact (on one or both finger pads) contacts_desired is "both", "left", "right", or "either"
Definition at line 339 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::freeze_arm | ( | self | ) |
tell the joint trajectory action to stop the arm where it is now
Definition at line 987 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::get_current_arm_angles | ( | self | ) |
use the joint_states_listener to get the arm angles
Definition at line 697 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::get_current_gripper_opening | ( | self | ) |
use the joint_states_listener to get the current gripper opening
Definition at line 710 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::get_current_wrist_pose_stamped | ( | self, | ||
frame = 'base_link' | ||||
| ) |
return the current pose of the wrist as a PoseStamped
Definition at line 704 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::get_find_gripper_contact_result | ( | self | ) |
get the final result from find_gripper_contact
Definition at line 371 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::get_find_gripper_contact_state | ( | self | ) |
get the state from find_gripper_contact
Definition at line 366 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::get_gripper_event_detector_state | ( | self | ) |
get the state from the gripper event detector
Definition at line 430 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::load_cartesian_controllers | ( | self | ) |
load the Cartesian controllers with the current set of params on the param server
Definition at line 1036 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::load_joint_controllers | ( | self | ) |
load the joint controller with the current set of params on the param server
Definition at line 1077 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::modify_move_arm_goal | ( | self, | ||
| goal, | ||||
| contact_info | ||||
| ) |
if move_arm_joint returns an error code of START_STATE_IN_COLLISION, we can try to add allowed contact regions to get it out of the start collisions (appears to already be done in C++)
Definition at line 519 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::move_arm_constrained | ( | self, | ||
constraint_in = None, |
||||
start_angles = None, |
||||
location = None, |
||||
blocking = 1, |
||||
compute_feasible_goal = True | ||||
| ) |
move the wrist to the desired location while keeping the current wrist orientation upright start angles are the joint angle start point for IK searches default start angles and location are for moving the arm to the side
Definition at line 544 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::move_arm_joint | ( | self, | ||
| joint_angles, | ||||
blocking = 1 | ||||
| ) |
use move_arm to get to a desired joint configuration in a collision-free way
Definition at line 495 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::move_arm_pose | ( | self, | ||
| pose_stamped, | ||||
start_angles = None, |
||||
blocking = 1 | ||||
| ) |
use move_arm to get to a desired pose in a collision-free way (really, run IK and then call move arm to move to the IK joint angles)
Definition at line 622 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::move_cartesian | ( | self, | ||
| goal_pose, | ||||
collision_aware = 0, |
||||
blocking = 1, |
||||
step_size = .005, |
||||
pos_thres = .02, |
||||
rot_thres = .1, |
||||
timeout = rospy.Duration(30.), |
||||
settling_time = rospy.Duration(1.) | ||||
| ) |
move in Cartesian space using the Cartesian controllers if collision_aware, checks whether the path could be collision-free, but uses the Cartesian controllers to execute it if blocking, waits for completion, otherwise returns after sending the goal step_size only used if collision aware (size of steps to check for collisions) pos_thres and rot_thres are thresholds within which the goal is declared reached times out and quits after timeout, and waits settling_time after the controllers declare that they're done (which is usually way too early)
Definition at line 805 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::move_cartesian_ik | ( | self, | ||
| goal_pose, | ||||
collision_aware = 0, |
||||
blocking = 1, |
||||
step_size = .005, |
||||
pos_thres = .02, |
||||
rot_thres = .1, |
||||
timeout = rospy.Duration(30.), |
||||
settling_time = rospy.Duration(0.25), |
||||
vel = .15 | ||||
| ) |
move in Cartesian space using IK Cartesian interpolation if collision_aware, quits if the path is not collision-free if blocking, waits for completion, otherwise returns after sending the goal step_size is the step size for interpolation pos_thres and rot_thres are thresholds within which the goal is declared reached times out and quits after timeout, and waits settling_time after the controllers declare that they're done
Definition at line 838 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::normalize_angle | ( | self, | ||
| angle, | ||||
| current_angle | ||||
| ) |
normalize an angle for a continuous joint so that it's the closest version of the angle to the current angle (not +-2*pi)
Definition at line 648 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::normalize_trajectory | ( | self, | ||
| trajectory | ||||
| ) |
normalize a trajectory (list of lists of joint angles), so that the desired angles are the nearest ones for the continuous joints (5 and 7)
Definition at line 637 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::pplist | ( | self, | ||
| list | ||||
| ) |
pretty-print list to string
Definition at line 1282 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::ppmat | ( | self, | ||
| mat | ||||
| ) |
pretty-print numpy matrix to string
Definition at line 1287 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::print_cartesian_pose | ( | self | ) |
print the current Cartesian pose of the gripper
Definition at line 1275 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::reload_cartesian_controllers | ( | self, | ||
set_params = 1 | ||||
| ) |
re-load the Cartesian controllers with new parameters (change cart_params before running)
Definition at line 1216 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::reload_joint_controllers | ( | self, | ||
fraction = 1. | ||||
| ) |
re-load the joint controller with gains a fraction of the defaults
Definition at line 1243 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::return_cartesian_pose | ( | self, | ||
frame = 'base_link' | ||||
| ) |
return the current Cartesian pose of the gripper
Definition at line 1269 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::send_move_arm_goal | ( | self, | ||
| goal, | ||||
blocking = 1 | ||||
| ) |
send a goal to move arm and wait for the result, modifying the goal if the start state is in collision
Definition at line 449 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::start_cartesian_controllers | ( | self | ) |
start the Cartesian controllers (need to be loaded already)
Definition at line 1151 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::start_gripper_controller | ( | self | ) |
start the gripper controller (needs to be loaded already)
Definition at line 1166 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::start_gripper_event_detector | ( | self, | ||
blocking = 0, |
||||
timeout = 15. | ||||
| ) |
start up gripper event detector to detect when an object hits the table or when someone is trying to take an object from the robot
Definition at line 405 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::start_gripper_grab | ( | self, | ||
hardness_gain = 0.035, |
||||
timeout = 10., |
||||
blocking = 1 | ||||
| ) |
opens and closes the gripper to determine how hard to grasp the object, then starts up slip servo mode
Definition at line 382 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::start_joint_controllers | ( | self | ) |
just start the joint controllers (need to be loaded already)
Definition at line 1137 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::stop_all_controllers | ( | self | ) |
stop all controllers
Definition at line 1180 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::stop_controllers | ( | self, | ||
stop_joint = 0, |
||||
stop_cartesian = 0, |
||||
stop_gripper = 0 | ||||
| ) |
stop controllers that are currently running
Definition at line 1185 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::switch_to_cartesian_mode | ( | self | ) |
stops the joint controllers, starts the Cartesian ones (both need to be loaded already)
Definition at line 1105 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::switch_to_joint_mode | ( | self | ) |
stops the Cartesian controllers, starts the joint ones (both need to be loaded already)
Definition at line 1121 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::unload_cartesian_controllers | ( | self | ) |
unload the Cartesian controllers (to re-load with new params)
Definition at line 1052 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::unload_joint_controllers | ( | self | ) |
unload the joint controller
Definition at line 1090 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::wait_cartesian_really_done | ( | self, | ||
| goal_pose, | ||||
pos_thres = .02, |
||||
rot_thres = .1, |
||||
timeout = 0., |
||||
settling_time = rospy.Duration(3.0) | ||||
| ) |
wait for either the Cartesian pose to get near enough to a goal pose or timeout (a ROS duration) is reached or settling_time (a ROS duration) after the Cartesian controllers report that they're done
Definition at line 779 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::wait_for_action_server | ( | self, | ||
| client, | ||||
| name | ||||
| ) |
wait for an action server to be ready
Definition at line 317 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::wait_for_service | ( | self, | ||
| name | ||||
| ) |
wait for a service to be ready
Definition at line 326 of file controller_manager.py.
| def pr2_gripper_reactive_approach::controller_manager::ControllerManager::wait_joint_trajectory_done | ( | self, | ||
timeout = rospy.Duration(0) | ||||
| ) |
wait for the joint trajectory action to finish (returns 1 if succeeded, 0 otherwise) timeout is a ROS duration; default (0) is infinite timeout returns 0 if timed out, 1 if succeeded
Definition at line 976 of file controller_manager.py.
Definition at line 269 of file controller_manager.py.
Definition at line 288 of file controller_manager.py.
Definition at line 259 of file controller_manager.py.
Definition at line 1027 of file controller_manager.py.
Definition at line 287 of file controller_manager.py.
Definition at line 289 of file controller_manager.py.
Definition at line 220 of file controller_manager.py.
Definition at line 264 of file controller_manager.py.
Definition at line 994 of file controller_manager.py.
Definition at line 238 of file controller_manager.py.
Definition at line 234 of file controller_manager.py.
Definition at line 236 of file controller_manager.py.
Definition at line 298 of file controller_manager.py.
Definition at line 214 of file controller_manager.py.
Definition at line 262 of file controller_manager.py.
Definition at line 873 of file controller_manager.py.
Definition at line 311 of file controller_manager.py.
Definition at line 272 of file controller_manager.py.
Definition at line 210 of file controller_manager.py.
Definition at line 206 of file controller_manager.py.
Definition at line 200 of file controller_manager.py.
Definition at line 242 of file controller_manager.py.
Definition at line 204 of file controller_manager.py.
Definition at line 254 of file controller_manager.py.
Definition at line 202 of file controller_manager.py.
Definition at line 183 of file controller_manager.py.
Definition at line 184 of file controller_manager.py.
Definition at line 181 of file controller_manager.py.
