object_manipulator::MechanismInterface Class Reference

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

#include <mechanism_interface.h>

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)
	Uses move arm to get to the desired set of joint values.
void	attemptTrajectory (std::string arm_name, const std::vector< std::vector< double > > &positions, bool unnormalize, float time_per_segment)
void	attemptTrajectory (std::string arm_name, const trajectory_msgs::JointTrajectory &trajectory, bool unnormalize)
	Uses the joint trajectory action to execute the desired trajectory.
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)
	Checks if a given arm state is valid with no collision operations or link paddings.
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.
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.
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 (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.
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.
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.
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)
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_)
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.
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_
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.
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 89 of file mechanism_interface.h.

Constructor & Destructor Documentation

object_manipulator::MechanismInterface::MechanismInterface ( )

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.

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 236 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 1127 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
	)

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 642 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 261 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 287 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::cachePlanningScene	(	const arm_navigation_msgs::OrderedCollisionOperations &	collision_operations,
		const std::vector< arm_navigation_msgs::LinkPadding > &	link_padding
	)			`[private]`

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 178 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 1228 of file mechanism_interface.cpp.

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

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

Definition at line 1412 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::checkController ( std::string controller )

Check to see if a controller is running.

Definition at line 1268 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 239 of file mechanism_interface.h.

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 410 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 1533 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 1433 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 1447 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::convertGraspableObjectComponentsToFrame	(	object_manipulation_msgs::GraspableObject &	object,
		std::string	frame_id
	)

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 1020 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::detachAllObjectsFromGripper ( std::string arm_name )

Detach and remove all objects attached to the gripper.

Definition at line 1151 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 1139 of file mechanism_interface.cpp.

std::vector< arm_navigation_msgs::LinkPadding > object_manipulator::MechanismInterface::fingertipPadding	(	std::string	arm_name,
		double	pad
	)			`[static]`

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

Definition at line 1758 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 1333 of file mechanism_interface.cpp.

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 354 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 974 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 381 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 467 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 127 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 1094 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::getPlanningScene	(	const arm_navigation_msgs::OrderedCollisionOperations &	collision_operations,
		const std::vector< arm_navigation_msgs::LinkPadding > &	link_padding
	)

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 218 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::getRobotState ( arm_navigation_msgs::RobotState & state )

Gets the current robot state.

Definition at line 139 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::graspPostureQuery	(	std::string	arm_name,
		const object_manipulation_msgs::Grasp	grasp
	)

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

Definition at line 1185 of file mechanism_interface.cpp.

std::vector< arm_navigation_msgs::LinkPadding > object_manipulator::MechanismInterface::gripperPadding	(	std::string	arm_name,
		double	pad
	)			`[static]`

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

Definition at line 1773 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 1161 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 1391 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::modifyMoveArmGoal	(	arm_navigation_msgs::MoveArmGoal &	move_arm_goal,
		arm_navigation_msgs::ArmNavigationErrorCodes &	error_code,
		std::vector< arm_navigation_msgs::ContactInformation > &	contact_info_
	)

Definition at line 739 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 782 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 343 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 1619 of file mechanism_interface.cpp.

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

Definition at line 376 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 366 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 1493 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 1197 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 1576 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 1585 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 1717 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 1723 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::setCartesianPostureGoalToCurrentAngles ( std::string arm_name )

Set the desired posture to the current arm angles.

Definition at line 1750 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 324 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::startController ( std::string controller )

Start one controller.

Definition at line 1288 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::stopController ( std::string controller )

Stop one controller.

Definition at line 1309 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 1244 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::switchToCartesian ( std::string arm_name )

Switch one arm from joint to Cartesian control.

Definition at line 1373 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::switchToJoint ( std::string arm_name )

Switch one arm from Cartesian to joint control.

Definition at line 1384 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 1003 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 1034 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 1054 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 1606 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 924 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 270 of file mechanism_interface.cpp.

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

Just a convenience function.

Definition at line 358 of file mechanism_interface.h.

Member Data Documentation

ros::Publisher object_manipulator::MechanismInterface::attached_object_pub_ [private]

Publisher for attached objects.

Definition at line 102 of file mechanism_interface.h.

bool object_manipulator::MechanismInterface::cache_planning_scene_ [private]

Used to disable planning scene caching altogether.

Definition at line 125 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 113 of file mechanism_interface.h.

MultiArmTopicWrapper<std_msgs::Float64MultiArray> object_manipulator::MechanismInterface::cartesian_posture_pub_

Publisher for Cartesian posture commands.

Definition at line 212 of file mechanism_interface.h.

MultiArmTopicWrapper<geometry_msgs::PoseStamped> object_manipulator::MechanismInterface::cartesian_pub_

Publisher for Cartesian pose commands.

Definition at line 209 of file mechanism_interface.h.

ServiceWrapper<arm_navigation_msgs::GetStateValidity> object_manipulator::MechanismInterface::check_state_validity_client_

Client for service that checks state validity.

Definition at line 163 of file mechanism_interface.h.

arm_navigation_msgs::OrderedCollisionOperations object_manipulator::MechanismInterface::collision_operations_cache_ [private]

Caches the last requested collision operations.

Definition at line 116 of file mechanism_interface.h.

MultiArmServiceWrapper<kinematics_msgs::GetPositionFK> object_manipulator::MechanismInterface::fk_service_client_

Client for the FK service.

Definition at line 150 of file mechanism_interface.h.

ServiceWrapper<arm_navigation_msgs::GetRobotState> object_manipulator::MechanismInterface::get_robot_state_client_

Client for service that gets robot state.

Definition at line 175 of file mechanism_interface.h.

MultiArmServiceWrapper<object_manipulation_msgs::GraspStatus> object_manipulator::MechanismInterface::grasp_status_client_

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

Definition at line 160 of file mechanism_interface.h.

MultiArmActionWrapper<object_manipulation_msgs::GraspHandPostureExecutionAction> object_manipulator::MechanismInterface::hand_posture_client_

Action client for controlling the gripper for executing grasps.

Definition at line 201 of file mechanism_interface.h.

MultiArmServiceWrapper<kinematics_msgs::GetKinematicSolverInfo> object_manipulator::MechanismInterface::ik_query_client_

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

Definition at line 144 of file mechanism_interface.h.

MultiArmServiceWrapper<kinematics_msgs::GetConstraintAwarePositionIK> object_manipulator::MechanismInterface::ik_service_client_

Client for the IK service.

Definition at line 147 of file mechanism_interface.h.

MultiArmServiceWrapper<arm_navigation_msgs::GetMotionPlan> object_manipulator::MechanismInterface::interpolated_ik_service_client_

Client for the Interpolated IK service.

Definition at line 153 of file mechanism_interface.h.

MultiArmServiceWrapper<interpolated_ik_motion_planner::SetInterpolatedIKMotionPlanParams> object_manipulator::MechanismInterface::interpolated_ik_set_params_client_

Client for setting the params of the Interpolated IK service.

Definition at line 157 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 109 of file mechanism_interface.h.

std::string object_manipulator::MechanismInterface::joint_states_topic_ [private]

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

Definition at line 105 of file mechanism_interface.h.

ServiceWrapper<arm_navigation_msgs::FilterJointTrajectory> object_manipulator::MechanismInterface::joint_trajectory_normalizer_service_

Client for the joint trajectory normalizer service.

Definition at line 166 of file mechanism_interface.h.

std::vector<arm_navigation_msgs::LinkPadding> object_manipulator::MechanismInterface::link_padding_cache_ [private]

Caches the last requested link padding.

Definition at line 119 of file mechanism_interface.h.

ServiceWrapper<pr2_mechanism_msgs::ListControllers> object_manipulator::MechanismInterface::list_controllers_service_

Client for the list controllers service.