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, 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 (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::CollisionModels &	getCollisionModels ()
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::KinematicState *	getPlanningSceneState () const
void	getRobotState (arm_navigation_msgs::RobotState &state)
	Gets the current robot state.
bool	graspPostureQuery (std::string arm_name, const manipulation_msgs::Grasp grasp)
	Queries the hand if a grasp is currently being correctly executed.
void	handPostureGraspAction (std::string arm_name, const 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::KinematicState *	planning_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

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.

object_manipulator::MechanismInterface::~MechanismInterface ( ) [inline]

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.

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 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.

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

Call the move arm action client's cancelGoal function.

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

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

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.

bool object_manipulator::MechanismInterface::clearMoveArmGoals ( double timeout = 30 )

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.

void object_manipulator::MechanismInterface::convertGraspableObjectComponentsToFrame	(	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 1082 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 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.

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 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.

planning_environment::CollisionModels& object_manipulator::MechanismInterface::getCollisionModels ( void ) [inline]

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.

actionlib::SimpleClientGoalState object_manipulator::MechanismInterface::getMoveArmState ( std::string arm_name )

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.

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

planning_models::KinematicState* object_manipulator::MechanismInterface::getPlanningSceneState ( ) const [inline]

Definition at line 238 of file mechanism_interface.h.

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

Gets the current robot state.

Definition at line 141 of file mechanism_interface.cpp.

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

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

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

void object_manipulator::MechanismInterface::handPostureGraspAction	(	std::string	arm_name,
		const 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.

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_
	)

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.

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

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.

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

Start one controller.

Definition at line 1350 of file mechanism_interface.cpp.

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

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.

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

Switch one arm from joint to Cartesian control.

Definition at line 1435 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 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

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

Publisher for attached objects.

Definition at line 104 of file mechanism_interface.h.

bool object_manipulator::MechanismInterface::cache_planning_scene_ [private]

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.

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

Publisher for Cartesian posture commands.

Definition at line 217 of file mechanism_interface.h.

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

Publisher for Cartesian pose commands.

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

planning_environment::CollisionModels object_manipulator::MechanismInterface::cm_ [private]

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

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

Client for the FK service.

Definition at line 155 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 180 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 165 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 206 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 149 of file mechanism_interface.h.

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

Client for the IK service.

Definition at line 152 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 158 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 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.

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 107 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 171 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 121 of file mechanism_interface.h.

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

Client for the list controllers service.