Public Member Functions | Static Public Member Functions | Public Attributes | Private Member Functions | Private Attributes
object_manipulator::MechanismInterface Class Reference

A collection of ROS service and action clients needed for grasp execution. More...

#include <mechanism_interface.h>

List of all members.

Public Member Functions

trajectory_msgs::JointTrajectory assembleJointTrajectory (std::string arm_name, const std::vector< std::vector< double > > &positions, float time_per_segment)
 Convenience function for assembly joint trajectory messages.
void attachObjectToGripper (std::string arm_name, std::string object_collision_name)
 Attaches a given object to the gripper.
bool attemptMoveArmToGoal (std::string arm_name, const std::vector< double > &desired_joint_values, const arm_navigation_msgs::OrderedCollisionOperations &collision_operations, const std::vector< arm_navigation_msgs::LinkPadding > &link_padding, int max_tries=5, bool reset_map_if_stuck=true, double timeout=60)
 Uses move arm to get to the desired set of joint values.
void attemptTrajectory (std::string arm_name, const trajectory_msgs::JointTrajectory &trajectory, bool unnormalize)
 Uses the joint trajectory action to execute the desired trajectory.
void attemptTrajectory (std::string arm_name, const std::vector< std::vector< double > > &positions, bool unnormalize, float time_per_segment)
void cancelMoveArmGoal (std::string arm_name)
 Call the move arm action client's cancelGoal function.
std::string cartesianControllerName (std::string arm_name)
 Get the name of the Cartesian controller for arm_name, returns an empty string if not found.
bool checkController (std::string controller)
 Check to see if a controller is running.
bool checkStateValidity (std::string arm_name, const std::vector< double > &joint_values, const arm_navigation_msgs::OrderedCollisionOperations &collision_operations, const std::vector< arm_navigation_msgs::LinkPadding > &link_padding)
 Checks if a given arm state is valid; joint_values must contain values for all joints of the arm.
bool checkStateValidity (std::string arm_name, const std::vector< double > &joint_values)
 Checks if a given arm state is valid with no collision operations or link paddings.
bool clearMoveArmGoals (double timeout=30)
 Cancel any currently-running move_arm goals (returns false if timed out, otherwise true)
geometry_msgs::PoseStamped clipDesiredPose (const geometry_msgs::PoseStamped &current_pose, const geometry_msgs::PoseStamped &desired_pose, double clip_dist, double clip_angle, double &resulting_clip_fraction)
 Clip a desired pose to be no more than clip_dist/clip_angle away from the given pose.
geometry_msgs::PoseStamped clipDesiredPose (std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, double clip_dist, double clip_angle, double &resulting_clip_fraction)
 Clip a desired pose to be no more than clip_dist/clip_angle away from the current gripper pose.
geometry_msgs::PoseStamped clipDesiredPose (std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, double clip_dist, double clip_angle, double &resulting_clip_fraction, const std::vector< double > &goal_posture_suggestion, std::vector< double > &clipped_posture_goal)
 Clip a desired pose to be no more than clip_dist/clip_angle away from the current gripper pose.
void convertGraspableObjectComponentsToFrame (object_manipulation_msgs::GraspableObject &object, std::string frame_id)
void detachAllObjectsFromGripper (std::string arm_name)
 Detach and remove all objects attached to the gripper.
void detachAndAddBackObjectsAttachedToGripper (std::string arm_name, std::string object_collision_name)
 Detached the indicated object from the gripper and adds it back in as a non-attached object.
bool getArmAngles (std::string arm_name, std::vector< double > &arm_angles)
 Get the current angles for arm_name.
planning_environment::CollisionModelsgetCollisionModels ()
bool getFK (std::string arm_name, std::vector< double > positions, geometry_msgs::PoseStamped &pose_stamped)
 Computes an FK solution.
geometry_msgs::PoseStamped getGripperPose (std::string arm_name, std::string frame_id)
 Gets the current pose of the gripper in the frame_id specified in the gripper_pose.header.
bool getIKForPose (std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, kinematics_msgs::GetConstraintAwarePositionIK::Response &ik_response, const arm_navigation_msgs::OrderedCollisionOperations &collision_operations, const std::vector< arm_navigation_msgs::LinkPadding > &link_padding)
 Computes an IK solution for a desired pose.
int getInterpolatedIK (std::string arm_name, geometry_msgs::PoseStamped start_pose, geometry_msgs::Vector3Stamped direction, float desired_trajectory_length, const std::vector< double > &seed_joint_position, const sensor_msgs::JointState &joint_state, const arm_navigation_msgs::OrderedCollisionOperations &collision_operations, const std::vector< arm_navigation_msgs::LinkPadding > &link_padding, bool reverse_trajectory, trajectory_msgs::JointTrajectory &trajectory, float &actual_trajectory_length)
 Computes an interpolated IK path between two poses.
std::vector< std::string > getJointNames (std::string arm_name)
 Returns the joint names for the arm we are using.
actionlib::SimpleClientGoalState getMoveArmState (std::string arm_name)
 Return the move arm action client's current state.
geometry_msgs::PoseStamped getObjectPoseForGrasp (std::string arm_name, const geometry_msgs::Pose &grasp_pose)
 Given a grasp pose relative to the wrist roll link, returns the current pose of the grasped object.
void getPlanningScene (const arm_navigation_msgs::OrderedCollisionOperations &collision_operations, const std::vector< arm_navigation_msgs::LinkPadding > &link_padding)
planning_models::KinematicStategetPlanningSceneState () const
void getRobotState (arm_navigation_msgs::RobotState &state)
 Gets the current robot state.
bool graspPostureQuery (std::string arm_name, const object_manipulation_msgs::Grasp grasp)
 Queries the hand if a grasp is currently being correctly executed.
void handPostureGraspAction (std::string arm_name, const object_manipulation_msgs::Grasp &grasp, int goal, float max_contact_force)
 Requests the hand to pre-grasp, grasp or release.
std::string jointControllerName (std::string arm_name)
 Get the name of the joint controller for arm_name, returns an empty string if not found.
 MechanismInterface ()
 Publisher for changing Cartesian gains.
void modifyMoveArmGoal (arm_navigation_msgs::MoveArmGoal &move_arm_goal, arm_navigation_msgs::ArmNavigationErrorCodes &error_code, std::vector< arm_navigation_msgs::ContactInformation > &contact_info_)
 Modify a MoveArmGoal to exclude an area around the returned collision points.
bool moveArmConstrained (std::string arm_name, const geometry_msgs::PoseStamped &commanded_pose, const arm_navigation_msgs::OrderedCollisionOperations &collision_operations, const std::vector< arm_navigation_msgs::LinkPadding > &link_padding, const arm_navigation_msgs::Constraints &path_constraints, const double &redundancy=0.0, const bool &compute_viable_pose=true)
 Uses move arm to a desired pose while keeping the object in the hand level.
bool moveArmToPose (std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, const arm_navigation_msgs::OrderedCollisionOperations &collision_operations, const std::vector< arm_navigation_msgs::LinkPadding > &link_padding)
 Uses move arm to a predefined arm position to the front, where the object can be transferred to the other arm.
int moveArmToPoseCartesian (std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, ros::Duration timeout, double dist_tol=0.001, double angle_tol=0.05, double clip_dist=.02, double clip_angle=.16, double overshoot_dist=.005, double overshoot_angle=.087, double timestep=0.1, const std::vector< double > &goal_posture_suggestion=std::vector< double >())
 Ask the wrist to go incrementally towards a PoseStamped using the Cartesian controller until it gets there.
geometry_msgs::Vector3 negate (const geometry_msgs::Vector3 &vec)
geometry_msgs::Vector3 normalize (const geometry_msgs::Vector3 &vec)
 Just a convenience function.
geometry_msgs::PoseStamped overshootDesiredPose (std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, double overshoot_dist, double overshoot_angle, double dist_tol, double angle_tol)
 Overshoot a desired pose by overshoot_dist and overshoot_angle.
bool pointHeadAction (const geometry_msgs::PointStamped &target, std::string pointing_frame="/narrow_stereo_optical_frame", bool wait_for_result=true)
 Requests the head to point at a specific position.
void poseDists (geometry_msgs::Pose start, geometry_msgs::Pose end, double &pos_dist, double &angle)
 Euclidean distance/angle between two Pose messages.
void positionAndAngleDist (Eigen::Affine3d start, Eigen::Affine3d end, double &pos_dist, double &angle, Eigen::Vector3d &axis, Eigen::Vector3d &direction)
 Euclidean distance/angle/axis/direction between two Eigen::Affine3ds.
void sendCartesianPoseCommand (std::string arm_name, geometry_msgs::PoseStamped desired_pose)
 Set a new desired PoseStamped for the Cartesian controller for arm_name.
void sendCartesianPostureCommand (std::string arm_name, std::vector< double > arm_angles)
 Set a new desired posture for the Cartesian controller for arm_name.
void setCartesianPostureGoalToCurrentAngles (std::string arm_name)
 Set the desired posture to the current arm angles.
bool startController (std::string controller)
 Start one controller.
bool stopController (std::string controller)
 Stop one controller.
bool switchControllers (std::string start_controller, std::string stop_controller)
 Switch one controller for another.
bool switchToCartesian (std::string arm_name)
 Switch one arm from joint to Cartesian control.
bool switchToJoint (std::string arm_name)
 Switch one arm from Cartesian to joint control.
void transformPointCloud (std::string target_frame, const sensor_msgs::PointCloud &cloud_in, sensor_msgs::PointCloud &cloud_out)
 Transforms a cloud from one frame to another; just passes through to the tf::Listener.
geometry_msgs::PoseStamped transformPose (const std::string target_frame, const geometry_msgs::PoseStamped &stamped_in)
 Transforms a pose from one frame to another; just passes through to the tf::Listener.
bool translateGripper (std::string arm_name, const geometry_msgs::Vector3Stamped &direction, arm_navigation_msgs::OrderedCollisionOperations ord, const std::vector< arm_navigation_msgs::LinkPadding > &link_padding, float requested_distance, float min_distance, float &actual_distance)
 Translates the gripper from its current pose to a new one using interpolated ik.
int translateGripperCartesian (std::string arm_name, const geometry_msgs::Vector3Stamped &direction, ros::Duration timeout, double dist_tol=.001, double angle_tol=.05, double clip_dist=.02, double clip_angle=.16, double overshoot_dist=0.005, double overshoot_angle=0.087, double timestep=0.1)
 Translate the gripper by direction using the Cartesian controllers.
geometry_msgs::PoseStamped translateGripperPose (geometry_msgs::Vector3Stamped translation, geometry_msgs::PoseStamped start_pose, std::string arm_id)
 Computes the gripper pose transformed by a certain translation.
void unnormalizeTrajectory (std::string arm_name, const trajectory_msgs::JointTrajectory &input_trajectory, trajectory_msgs::JointTrajectory &normalized_trajectory)
 Normalizes a trajectory (joint angle wrap-around)
float vectorLen (const geometry_msgs::Vector3 &vec)
 Just a convenience function.
 ~MechanismInterface ()

Static Public Member Functions

static std::vector
< arm_navigation_msgs::LinkPadding
fingertipPadding (std::string arm_name, double pad)
 Returns the link padding vector for setting the desired padding to gripper fingertip links.
static std::vector
< arm_navigation_msgs::LinkPadding
gripperPadding (std::string arm_name, double pad)
 Returns the link padding vector for setting the desired padding to gripper touch links.

Public Attributes

MultiArmTopicWrapper
< std_msgs::Float64MultiArray > 
cartesian_posture_pub_
 Publisher for Cartesian posture commands.
MultiArmTopicWrapper
< geometry_msgs::PoseStamped > 
cartesian_pub_
 Publisher for Cartesian pose commands.
ServiceWrapper
< arm_navigation_msgs::GetStateValidity
check_state_validity_client_
 Client for service that checks state validity.
MultiArmServiceWrapper
< kinematics_msgs::GetPositionFK > 
fk_service_client_
 Client for the FK service.
ServiceWrapper
< arm_navigation_msgs::GetRobotState
get_robot_state_client_
 Client for service that gets robot state.
MultiArmServiceWrapper
< object_manipulation_msgs::GraspStatus
grasp_status_client_
 Client for service that queries if a graps is currently active.
MultiArmActionWrapper
< object_manipulation_msgs::GraspHandPostureExecutionAction
hand_posture_client_
 Action client for controlling the gripper for executing grasps.
MultiArmServiceWrapper
< kinematics_msgs::GetKinematicSolverInfo > 
ik_query_client_
 Client for IK information query that tells us the list of joints to work on.
MultiArmServiceWrapper
< kinematics_msgs::GetConstraintAwarePositionIK > 
ik_service_client_
 Client for the IK service.
MultiArmServiceWrapper
< arm_navigation_msgs::GetMotionPlan
interpolated_ik_service_client_
 Client for the Interpolated IK service.
MultiArmServiceWrapper
< interpolated_ik_motion_planner::SetInterpolatedIKMotionPlanParams
interpolated_ik_set_params_client_
 Client for setting the params of the Interpolated IK service.
ServiceWrapper
< arm_navigation_msgs::FilterJointTrajectory
joint_trajectory_normalizer_service_
 Client for the joint trajectory normalizer service.
ServiceWrapper
< pr2_mechanism_msgs::ListControllers > 
list_controllers_service_
 Client for the list controllers service.
MultiArmActionWrapper
< arm_navigation_msgs::MoveArmAction
move_arm_action_client_
 Action client for move_arm.
ActionWrapper
< pr2_controllers_msgs::PointHeadAction
point_head_action_client_
 Action client for pointing the head at a target.
MultiArmActionWrapper
< object_manipulation_msgs::ReactiveGraspAction
reactive_grasp_action_client_
 Action client for reactive grasping.
MultiArmActionWrapper
< object_manipulation_msgs::ReactiveLiftAction
reactive_lift_action_client_
 Action client for reactive lifting.
MultiArmActionWrapper
< object_manipulation_msgs::ReactivePlaceAction
reactive_place_action_client_
 Action client for reactive placing.
ServiceWrapper< std_srvs::Empty > reset_collision_map_service_
 Client for the service that resets the collision map.
ServiceWrapper
< arm_navigation_msgs::SetPlanningSceneDiff
set_planning_scene_diff_service_
 Client for the service that gets the planning scene.
ServiceWrapper
< pr2_mechanism_msgs::SwitchController > 
switch_controller_service_
 Client for the switch controller service.
MultiArmActionWrapper
< pr2_controllers_msgs::JointTrajectoryAction
traj_action_client_
 Action client for executing a joint trajectory directly, without move arm.

Private Member Functions

bool cachePlanningScene (const arm_navigation_msgs::OrderedCollisionOperations &collision_operations, const std::vector< arm_navigation_msgs::LinkPadding > &link_padding)
 Checks if the planning scene is the same as the last one requested, to avoid unnecessary calls.
bool callSwitchControllers (std::vector< std::string > start_controllers, std::vector< std::string > stop_controllers)
 Calls the switch_controllers service.
void setInterpolatedIKParams (std::string arm_name, int num_steps, int collision_check_resolution, bool start_from_end)
 Sets the parameters for the interpolated IK server.

Private Attributes

ros::Publisher attached_object_pub_
 Publisher for attached objects.
bool cache_planning_scene_
 Used to disable planning scene caching altogether.
std::map< std::string,
std::string > 
cartesian_controller_names_
planning_environment::CollisionModels cm_
arm_navigation_msgs::OrderedCollisionOperations collision_operations_cache_
 Caches the last requested collision operations.
std::map< std::string,
std::string > 
joint_controller_names_
std::string joint_states_topic_
 Name of the topic on which JointState messages come (for the current arm angles)
std::vector
< arm_navigation_msgs::LinkPadding
link_padding_cache_
 Caches the last requested link padding.
tf::TransformListener listener_
 Transform listener.
bool planning_scene_cache_empty_
 Keeps track of whether planning scene caching has been called at all.
planning_models::KinematicStateplanning_scene_state_
ros::NodeHandle priv_nh_
 The private namespace node handle.
ros::NodeHandle root_nh_
 The root namespace node handle.

Detailed Description

A collection of ROS service and action clients needed for grasp execution.

Definition at line 91 of file mechanism_interface.h.


Constructor & Destructor Documentation

Publisher for changing Cartesian gains.

Initializes all clients, then calls getIKInformation() and sets default interpolated IK params

Definition at line 83 of file mechanism_interface.cpp.

Definition at line 228 of file mechanism_interface.h.


Member Function Documentation

trajectory_msgs::JointTrajectory object_manipulator::MechanismInterface::assembleJointTrajectory ( std::string  arm_name,
const std::vector< std::vector< double > > &  positions,
float  time_per_segment 
)

Convenience function for assembly joint trajectory messages.

Definition at line 244 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::attachObjectToGripper ( std::string  arm_name,
std::string  object_collision_name 
)

Attaches a given object to the gripper.

Definition at line 1189 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::attemptMoveArmToGoal ( std::string  arm_name,
const std::vector< double > &  desired_joint_values,
const arm_navigation_msgs::OrderedCollisionOperations collision_operations,
const std::vector< arm_navigation_msgs::LinkPadding > &  link_padding,
int  max_tries = 5,
bool  reset_map_if_stuck = true,
double  timeout = 60 
)

Uses move arm to get to the desired set of joint values.

If move arm is still stuck, then nothing will get it out of here

Definition at line 702 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::attemptTrajectory ( std::string  arm_name,
const trajectory_msgs::JointTrajectory &  trajectory,
bool  unnormalize 
)

Uses the joint trajectory action to execute the desired trajectory.

Definition at line 295 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::attemptTrajectory ( std::string  arm_name,
const std::vector< std::vector< double > > &  positions,
bool  unnormalize,
float  time_per_segment 
)

Convenience function that just gets the joint values for the trajectory, and assumes that we are using the joint returned by the IK server in that order

Definition at line 269 of file mechanism_interface.cpp.

Checks if the planning scene is the same as the last one requested, to avoid unnecessary calls.

Returns true if the collision operations and link paddings are identical to the last ones requested.

Definition at line 180 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::callSwitchControllers ( std::vector< std::string >  start_controllers,
std::vector< std::string >  stop_controllers 
) [private]

Calls the switch_controllers service.

Definition at line 1290 of file mechanism_interface.cpp.

Call the move arm action client's cancelGoal function.

Definition at line 650 of file mechanism_interface.cpp.

Get the name of the Cartesian controller for arm_name, returns an empty string if not found.

Definition at line 1474 of file mechanism_interface.cpp.

Check to see if a controller is running.

Definition at line 1330 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::checkStateValidity ( std::string  arm_name,
const std::vector< double > &  joint_values,
const arm_navigation_msgs::OrderedCollisionOperations collision_operations,
const std::vector< arm_navigation_msgs::LinkPadding > &  link_padding 
)

Checks if a given arm state is valid; joint_values must contain values for all joints of the arm.

Definition at line 418 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::checkStateValidity ( std::string  arm_name,
const std::vector< double > &  joint_values 
) [inline]

Checks if a given arm state is valid with no collision operations or link paddings.

Definition at line 258 of file mechanism_interface.h.

Cancel any currently-running move_arm goals (returns false if timed out, otherwise true)

Definition at line 661 of file mechanism_interface.cpp.

geometry_msgs::PoseStamped object_manipulator::MechanismInterface::clipDesiredPose ( const geometry_msgs::PoseStamped &  current_pose,
const geometry_msgs::PoseStamped &  desired_pose,
double  clip_dist,
double  clip_angle,
double &  resulting_clip_fraction 
)

Clip a desired pose to be no more than clip_dist/clip_angle away from the given pose.

Definition at line 1509 of file mechanism_interface.cpp.

geometry_msgs::PoseStamped object_manipulator::MechanismInterface::clipDesiredPose ( std::string  arm_name,
const geometry_msgs::PoseStamped &  desired_pose,
double  clip_dist,
double  clip_angle,
double &  resulting_clip_fraction 
)

Clip a desired pose to be no more than clip_dist/clip_angle away from the current gripper pose.

Definition at line 1495 of file mechanism_interface.cpp.

geometry_msgs::PoseStamped object_manipulator::MechanismInterface::clipDesiredPose ( std::string  arm_name,
const geometry_msgs::PoseStamped &  desired_pose,
double  clip_dist,
double  clip_angle,
double &  resulting_clip_fraction,
const std::vector< double > &  goal_posture_suggestion,
std::vector< double > &  clipped_posture_goal 
)

Clip a desired pose to be no more than clip_dist/clip_angle away from the current gripper pose.

Definition at line 1595 of file mechanism_interface.cpp.

Converts all the internal components of a GraspableObject to the desired frame, and sets that frame as the reference_frame_id of that object. Does NOT convert the SceneRegion component which is camera frame by definition.

Definition at line 1082 of file mechanism_interface.cpp.

Detach and remove all objects attached to the gripper.

Definition at line 1213 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::detachAndAddBackObjectsAttachedToGripper ( std::string  arm_name,
std::string  object_collision_name 
)

Detached the indicated object from the gripper and adds it back in as a non-attached object.

Definition at line 1201 of file mechanism_interface.cpp.

Returns the link padding vector for setting the desired padding to gripper fingertip links.

Definition at line 1820 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::getArmAngles ( std::string  arm_name,
std::vector< double > &  arm_angles 
)

Get the current angles for arm_name.

Definition at line 1395 of file mechanism_interface.cpp.

Definition at line 234 of file mechanism_interface.h.

bool object_manipulator::MechanismInterface::getFK ( std::string  arm_name,
std::vector< double >  positions,
geometry_msgs::PoseStamped &  pose_stamped 
)

Computes an FK solution.

Definition at line 362 of file mechanism_interface.cpp.

geometry_msgs::PoseStamped object_manipulator::MechanismInterface::getGripperPose ( std::string  arm_name,
std::string  frame_id 
)

Gets the current pose of the gripper in the frame_id specified in the gripper_pose.header.

Current gripper pose is returned in the requested frame_id.

Definition at line 1036 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::getIKForPose ( std::string  arm_name,
const geometry_msgs::PoseStamped &  desired_pose,
kinematics_msgs::GetConstraintAwarePositionIK::Response &  ik_response,
const arm_navigation_msgs::OrderedCollisionOperations collision_operations,
const std::vector< arm_navigation_msgs::LinkPadding > &  link_padding 
)

Computes an IK solution for a desired pose.

Definition at line 389 of file mechanism_interface.cpp.

int object_manipulator::MechanismInterface::getInterpolatedIK ( std::string  arm_name,
geometry_msgs::PoseStamped  start_pose,
geometry_msgs::Vector3Stamped  direction,
float  desired_trajectory_length,
const std::vector< double > &  seed_joint_position,
const sensor_msgs::JointState &  joint_state,
const arm_navigation_msgs::OrderedCollisionOperations collision_operations,
const std::vector< arm_navigation_msgs::LinkPadding > &  link_padding,
bool  reverse_trajectory,
trajectory_msgs::JointTrajectory &  trajectory,
float &  actual_trajectory_length 
)

Computes an interpolated IK path between two poses.

If starting_from_end is set to true, the Interpolated IK server will be set to start from end as well, and the resulting trajectory is copied into the result starting from the end until a failed step is encountered.

  • seed_joint_position is a seed to be use for IK for the joints we are planning on. Pass an empty vector if you don't have a seed you want to use
  • joint_state is a list of values to be used for joints that are not part of our plan. For example, use this to specify if you want the plan done with the gripper open or closed. If you don't specify a joint in here, the current value of that joint will be used by the Interpolated IK server.

Definition at line 475 of file mechanism_interface.cpp.

std::vector< std::string > object_manipulator::MechanismInterface::getJointNames ( std::string  arm_name)

Returns the joint names for the arm we are using.

For now, just calls the IK Info service each time. In the future, we might do some caching in here.

Definition at line 129 of file mechanism_interface.cpp.

Return the move arm action client's current state.

Definition at line 655 of file mechanism_interface.cpp.

geometry_msgs::PoseStamped object_manipulator::MechanismInterface::getObjectPoseForGrasp ( std::string  arm_name,
const geometry_msgs::Pose grasp_pose 
)

Given a grasp pose relative to the wrist roll link, returns the current pose of the grasped object.

Object pose is obtained by multiplying the grasp_pose with the current pose of the wrist roll link of the given arm. Result is given in base_link frame.

Definition at line 1156 of file mechanism_interface.cpp.

Sends the requsted collision operations and link padding to the environment server as a diff from the current planning scene on the server

Definition at line 220 of file mechanism_interface.cpp.

Definition at line 238 of file mechanism_interface.h.

Gets the current robot state.

Definition at line 141 of file mechanism_interface.cpp.

Queries the hand if a grasp is currently being correctly executed.

Definition at line 1247 of file mechanism_interface.cpp.

Returns the link padding vector for setting the desired padding to gripper touch links.

Definition at line 1835 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::handPostureGraspAction ( std::string  arm_name,
const object_manipulation_msgs::Grasp grasp,
int  goal,
float  max_contact_force 
)

Requests the hand to pre-grasp, grasp or release.

Definition at line 1223 of file mechanism_interface.cpp.

std::string object_manipulator::MechanismInterface::jointControllerName ( std::string  arm_name)

Get the name of the joint controller for arm_name, returns an empty string if not found.

Definition at line 1453 of file mechanism_interface.cpp.

Modify a MoveArmGoal to exclude an area around the returned collision points.

Definition at line 801 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::moveArmConstrained ( std::string  arm_name,
const geometry_msgs::PoseStamped &  commanded_pose,
const arm_navigation_msgs::OrderedCollisionOperations collision_operations,
const std::vector< arm_navigation_msgs::LinkPadding > &  link_padding,
const arm_navigation_msgs::Constraints path_constraints,
const double &  redundancy = 0.0,
const bool compute_viable_pose = true 
)

Uses move arm to a desired pose while keeping the object in the hand level.

Definition at line 844 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::moveArmToPose ( std::string  arm_name,
const geometry_msgs::PoseStamped &  desired_pose,
const arm_navigation_msgs::OrderedCollisionOperations collision_operations,
const std::vector< arm_navigation_msgs::LinkPadding > &  link_padding 
)

Uses move arm to a predefined arm position to the front, where the object can be transferred to the other arm.

Definition at line 351 of file mechanism_interface.cpp.

int object_manipulator::MechanismInterface::moveArmToPoseCartesian ( std::string  arm_name,
const geometry_msgs::PoseStamped &  desired_pose,
ros::Duration  timeout,
double  dist_tol = 0.001,
double  angle_tol = 0.05,
double  clip_dist = .02,
double  clip_angle = .16,
double  overshoot_dist = .005,
double  overshoot_angle = .087,
double  timestep = 0.1,
const std::vector< double > &  goal_posture_suggestion = std::vector<double>() 
)

Ask the wrist to go incrementally towards a PoseStamped using the Cartesian controller until it gets there.

Definition at line 1681 of file mechanism_interface.cpp.

geometry_msgs::Vector3 object_manipulator::MechanismInterface::negate ( const geometry_msgs::Vector3 &  vec) [inline]

Definition at line 406 of file mechanism_interface.h.

geometry_msgs::Vector3 object_manipulator::MechanismInterface::normalize ( const geometry_msgs::Vector3 &  vec) [inline]

Just a convenience function.

Definition at line 396 of file mechanism_interface.h.

geometry_msgs::PoseStamped object_manipulator::MechanismInterface::overshootDesiredPose ( std::string  arm_name,
const geometry_msgs::PoseStamped &  desired_pose,
double  overshoot_dist,
double  overshoot_angle,
double  dist_tol,
double  angle_tol 
)

Overshoot a desired pose by overshoot_dist and overshoot_angle.

Definition at line 1555 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::pointHeadAction ( const geometry_msgs::PointStamped &  target,
std::string  pointing_frame = "/narrow_stereo_optical_frame",
bool  wait_for_result = true 
)

Requests the head to point at a specific position.

Definition at line 1259 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::poseDists ( geometry_msgs::Pose  start,
geometry_msgs::Pose  end,
double &  pos_dist,
double &  angle 
)

Euclidean distance/angle between two Pose messages.

Definition at line 1638 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::positionAndAngleDist ( Eigen::Affine3d  start,
Eigen::Affine3d  end,
double &  pos_dist,
double &  angle,
Eigen::Vector3d &  axis,
Eigen::Vector3d &  direction 
)

Euclidean distance/angle/axis/direction between two Eigen::Affine3ds.

Definition at line 1647 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::sendCartesianPoseCommand ( std::string  arm_name,
geometry_msgs::PoseStamped  desired_pose 
)

Set a new desired PoseStamped for the Cartesian controller for arm_name.

Definition at line 1779 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::sendCartesianPostureCommand ( std::string  arm_name,
std::vector< double >  arm_angles 
)

Set a new desired posture for the Cartesian controller for arm_name.

Definition at line 1785 of file mechanism_interface.cpp.

Set the desired posture to the current arm angles.

Definition at line 1812 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::setInterpolatedIKParams ( std::string  arm_name,
int  num_steps,
int  collision_check_resolution,
bool  start_from_end 
) [private]

Sets the parameters for the interpolated IK server.

Definition at line 332 of file mechanism_interface.cpp.

Start one controller.

Definition at line 1350 of file mechanism_interface.cpp.

Stop one controller.

Definition at line 1371 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::switchControllers ( std::string  start_controller,
std::string  stop_controller 
)

Switch one controller for another.

Definition at line 1306 of file mechanism_interface.cpp.

Switch one arm from joint to Cartesian control.

Definition at line 1435 of file mechanism_interface.cpp.

Switch one arm from Cartesian to joint control.

Definition at line 1446 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::transformPointCloud ( std::string  target_frame,
const sensor_msgs::PointCloud cloud_in,
sensor_msgs::PointCloud cloud_out 
)

Transforms a cloud from one frame to another; just passes through to the tf::Listener.

Definition at line 1065 of file mechanism_interface.cpp.

geometry_msgs::PoseStamped object_manipulator::MechanismInterface::transformPose ( const std::string  target_frame,
const geometry_msgs::PoseStamped &  stamped_in 
)

Transforms a pose from one frame to another; just passes through to the tf::Listener.

Definition at line 1096 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::translateGripper ( std::string  arm_name,
const geometry_msgs::Vector3Stamped &  direction,
arm_navigation_msgs::OrderedCollisionOperations  ord,
const std::vector< arm_navigation_msgs::LinkPadding > &  link_padding,
float  requested_distance,
float  min_distance,
float &  actual_distance 
)

Translates the gripper from its current pose to a new one using interpolated ik.

Moves the gripper from its current pose to the one obtained by the specified translation.

Definition at line 1116 of file mechanism_interface.cpp.

int object_manipulator::MechanismInterface::translateGripperCartesian ( std::string  arm_name,
const geometry_msgs::Vector3Stamped &  direction,
ros::Duration  timeout,
double  dist_tol = .001,
double  angle_tol = .05,
double  clip_dist = .02,
double  clip_angle = .16,
double  overshoot_dist = 0.005,
double  overshoot_angle = 0.087,
double  timestep = 0.1 
)

Translate the gripper by direction using the Cartesian controllers.

Definition at line 1668 of file mechanism_interface.cpp.

geometry_msgs::PoseStamped object_manipulator::MechanismInterface::translateGripperPose ( geometry_msgs::Vector3Stamped  translation,
geometry_msgs::PoseStamped  start_pose,
std::string  arm_name 
)

Computes the gripper pose transformed by a certain translation.

The translation must be expressed either in the gripper frame or in the robot frame. The reason is the following: this function must work even without tf, so that it works for hypothetical situations, or situations in which the complete tf tree is unknown. Its only decision is thus whether to pre-multiply or post-multiply the pose with the given translation.

If the translation is in gripper frame, the pose gets post-multiplied by it. If the translation is in robot frame, the pose gets pre-multiplied by it.

The input pose is first transformed to the robot frame, then translated, then transformed back into the original frame.

Definition at line 986 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::unnormalizeTrajectory ( std::string  arm_name,
const trajectory_msgs::JointTrajectory &  input_trajectory,
trajectory_msgs::JointTrajectory &  normalized_trajectory 
)

Normalizes a trajectory (joint angle wrap-around)

Definition at line 278 of file mechanism_interface.cpp.

float object_manipulator::MechanismInterface::vectorLen ( const geometry_msgs::Vector3 &  vec) [inline]

Just a convenience function.

Definition at line 388 of file mechanism_interface.h.


Member Data Documentation

Publisher for attached objects.

Definition at line 104 of file mechanism_interface.h.

Used to disable planning scene caching altogether.

Definition at line 130 of file mechanism_interface.h.

std::map<std::string, std::string> object_manipulator::MechanismInterface::cartesian_controller_names_ [private]

Names for the arm controllers (only needed when used) Values are taken from the params arm_name_joint_controller and arm_name_cartesian_controller (for each arm_name)

Definition at line 115 of file mechanism_interface.h.

Publisher for Cartesian posture commands.

Definition at line 217 of file mechanism_interface.h.

Publisher for Cartesian pose commands.

Definition at line 214 of file mechanism_interface.h.

Client for service that checks state validity.

Definition at line 168 of file mechanism_interface.h.

Definition at line 123 of file mechanism_interface.h.

Caches the last requested collision operations.

Definition at line 118 of file mechanism_interface.h.

Client for the FK service.

Definition at line 155 of file mechanism_interface.h.

Client for service that gets robot state.

Definition at line 180 of file mechanism_interface.h.

Client for service that queries if a graps is currently active.

Definition at line 165 of file mechanism_interface.h.

Action client for controlling the gripper for executing grasps.

Definition at line 206 of file mechanism_interface.h.

Client for IK information query that tells us the list of joints to work on.

Definition at line 149 of file mechanism_interface.h.

Client for the IK service.

Definition at line 152 of file mechanism_interface.h.

Client for the Interpolated IK service.

Definition at line 158 of file mechanism_interface.h.

Client for setting the params of the Interpolated IK service.

Definition at line 162 of file mechanism_interface.h.

std::map<std::string, std::string> object_manipulator::MechanismInterface::joint_controller_names_ [private]

Names for the arm controllers (only needed when used) Values are taken from the params arm_name_joint_controller and arm_name_cartesian_controller (for each arm_name)

Definition at line 111 of file mechanism_interface.h.

Name of the topic on which JointState messages come (for the current arm angles)

Definition at line 107 of file mechanism_interface.h.

Client for the joint trajectory normalizer service.

Definition at line 171 of file mechanism_interface.h.

Caches the last requested link padding.

Definition at line 121 of file mechanism_interface.h.

Client for the list controllers service.

Definition at line 177 of file mechanism_interface.h.

Transform listener.

Definition at line 101 of file mechanism_interface.h.

Action client for move_arm.

Definition at line 200 of file mechanism_interface.h.

Keeps track of whether planning scene caching has been called at all.

Definition at line 127 of file mechanism_interface.h.

Definition at line 124 of file mechanism_interface.h.

Action client for pointing the head at a target.

Definition at line 209 of file mechanism_interface.h.

The private namespace node handle.

Definition at line 98 of file mechanism_interface.h.

Action client for reactive grasping.

Definition at line 191 of file mechanism_interface.h.

Action client for reactive lifting.

Definition at line 194 of file mechanism_interface.h.

Action client for reactive placing.

Definition at line 197 of file mechanism_interface.h.

Client for the service that resets the collision map.

Definition at line 186 of file mechanism_interface.h.

The root namespace node handle.

Definition at line 95 of file mechanism_interface.h.

Client for the service that gets the planning scene.

Definition at line 183 of file mechanism_interface.h.

Client for the switch controller service.

Definition at line 174 of file mechanism_interface.h.

Action client for executing a joint trajectory directly, without move arm.

Definition at line 203 of file mechanism_interface.h.


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


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04