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

#include <mechanism_interface.h>

Public Member Functions
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 motion_planning_msgs::OrderedCollisionOperations &collision_operations, const std::vector< motion_planning_msgs::LinkPadding > &link_padding)
	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)
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 motion_planning_msgs::OrderedCollisionOperations &collision_operations, const std::vector< motion_planning_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.
geometry_msgs::PoseStamped	clipDesiredPose (std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, double clip_dist, double clip_angle)
	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	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	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 motion_planning_msgs::OrderedCollisionOperations &collision_operations, const std::vector< motion_planning_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 motion_planning_msgs::OrderedCollisionOperations &collision_operations, const std::vector< motion_planning_msgs::LinkPadding > &link_padding, bool reverse_trajectory, trajectory_msgs::JointTrajectory &trajectory, float &actual_trajectory_length)
	Computes an interpolated IK path between two poses.
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.
bool	graspPostureQuery (std::string arm_name)
	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)
	Requests the hand to pre-grasp, grasp or release.
	MechanismInterface ()
	Initializes all clients, then calls getIKInformation() and sets default interpolated IK params.
void	modifyMoveArmGoal (move_arm_msgs::MoveArmGoal &move_arm_goal, motion_planning_msgs::ArmNavigationErrorCodes &error_code, std::vector< planning_environment_msgs::ContactInformation > &contact_info_)
bool	moveArmConstrained (std::string arm_name, const geometry_msgs::PoseStamped &commanded_pose, const motion_planning_msgs::OrderedCollisionOperations &collision_operations, const std::vector< motion_planning_msgs::LinkPadding > &link_padding, const motion_planning_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 motion_planning_msgs::OrderedCollisionOperations &collision_operations, const std::vector< motion_planning_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, double angle_tol, double clip_dist, double clip_angle, double timestep)
	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.
bool	pointHeadAction (const geometry_msgs::PointStamped &target, std::string pointing_frame="/narrow_stereo_optical_frame")
	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::eigen2_Transform3d start, Eigen::eigen2_Transform3d end, double &pos_dist, double &angle, Eigen::Vector3d &axis, Eigen::Vector3d &direction)
	Euclidean distance/angle/axis/direction between two Eigen::eigen2_Transform3ds.
void	sendCartesianPoseCommand (std::string arm_name, geometry_msgs::PoseStamped desired_pose, double clip_dist, double clip_angle)
	Set a new desired PoseStamped for the Cartesian controller for arm_name.
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 ()
	Switch both arms from joint to Cartesian control.
bool	switchToJoint ()
	Switch both arms 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, motion_planning_msgs::OrderedCollisionOperations ord, const std::vector< motion_planning_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, double angle_tol, double clip_dist, double clip_angle, double timestep)
	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.
float	vectorLen (const geometry_msgs::Vector3 &vec)
	Just a convenience function.
Static Public Member Functions
static std::vector < motion_planning_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 < motion_planning_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
ServiceWrapper < planning_environment_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.
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 < motion_planning_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 < motion_planning_msgs::FilterJointTrajectory >	joint_trajectory_normalizer_service_
	Client for the joint trajectory normalizer service.
ros::Publisher	left_cartesian_pub_
ServiceWrapper < pr2_mechanism_msgs::ListControllers >	list_controllers_service_
	Client for the list controllers service.
MultiArmActionWrapper < move_arm_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.
ros::Publisher	right_cartesian_pub_
	Publishers for Cartesian pose commands.
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	callSwitchControllers (std::vector< std::string > start_controllers, std::vector< std::string > stop_controllers)
	Calls the switch_controllers service.
std::vector< std::string >	getJointNames (std::string arm_name)
	Returns the joint names for the arm we are using.
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.
std::string	left_cartesian_controller_
std::string	left_joint_controller_
tf::TransformListener	listener_
	Transform listener.
ros::NodeHandle	priv_nh_
	The private namespace node handle.
std::string	right_cartesian_controller_
	Names of the joint and cartesian controllers.
std::string	right_joint_controller_
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 85 of file mechanism_interface.h.

Constructor & Destructor Documentation

object_manipulator::MechanismInterface::MechanismInterface ( )

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

Definition at line 73 of file mechanism_interface.cpp.

Member Function Documentation

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

Attaches a given object to the gripper.

Definition at line 942 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::attemptMoveArmToGoal	(	std::string	arm_name,
		const std::vector< double > &	desired_joint_values,
		const motion_planning_msgs::OrderedCollisionOperations &	collision_operations,
		const std::vector< motion_planning_msgs::LinkPadding > &	link_padding
	)

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

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

Definition at line 489 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 155 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 129 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 1027 of file mechanism_interface.cpp.

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

Check to see if a controller is running.

Definition at line 1067 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::checkStateValidity	(	std::string	arm_name,
		const std::vector< double > &	joint_values,
		const motion_planning_msgs::OrderedCollisionOperations &	collision_operations,
		const std::vector< motion_planning_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 287 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 193 of file mechanism_interface.h.

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

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

Definition at line 1135 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 835 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 954 of file mechanism_interface.cpp.

std::vector< motion_planning_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 1304 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 231 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 798 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 motion_planning_msgs::OrderedCollisionOperations &	collision_operations,
		const std::vector< motion_planning_msgs::LinkPadding > &	link_padding
	)

Computes an IK solution for a desired pose.

Definition at line 258 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 motion_planning_msgs::OrderedCollisionOperations &	collision_operations,
		const std::vector< motion_planning_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 specifiy 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 326 of file mechanism_interface.cpp.

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

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

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

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

Definition at line 989 of file mechanism_interface.cpp.

std::vector< motion_planning_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 1319 of file mechanism_interface.cpp.

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

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

Definition at line 966 of file mechanism_interface.cpp.

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

Definition at line 566 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::moveArmConstrained	(	std::string	arm_name,
		const geometry_msgs::PoseStamped &	commanded_pose,
		const motion_planning_msgs::OrderedCollisionOperations &	collision_operations,
		const std::vector< motion_planning_msgs::LinkPadding > &	link_padding,
		const motion_planning_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 609 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::moveArmToPose	(	std::string	arm_name,
		const geometry_msgs::PoseStamped &	desired_pose,
		const motion_planning_msgs::OrderedCollisionOperations &	collision_operations,
		const std::vector< motion_planning_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 219 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 = `.015`,
		double	angle_tol = `.09`,
		double	clip_dist = `.02`,
		double	clip_angle = `.16`,
		double	timestep = `0.1`
	)

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

Definition at line 1218 of file mechanism_interface.cpp.

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

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

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

Requests the head to point at a specific position.

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

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

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

Definition at line 1190 of file mechanism_interface.cpp.

void object_manipulator::MechanismInterface::sendCartesianPoseCommand	(	std::string	arm_name,
		geometry_msgs::PoseStamped	desired_pose,
		double	clip_dist = `0`,
		double	clip_angle = `0`
	)

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

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

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

Start one controller.

Definition at line 1072 of file mechanism_interface.cpp.

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

Stop one controller.

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

bool object_manipulator::MechanismInterface::switchToCartesian ( )

Switch both arms from joint to Cartesian control.

Definition at line 1117 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::switchToJoint ( )

Switch both arms from Cartesian to joint control.

Definition at line 1126 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 818 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 849 of file mechanism_interface.cpp.

bool object_manipulator::MechanismInterface::translateGripper	(	std::string	arm_name,
		const geometry_msgs::Vector3Stamped &	direction,
		motion_planning_msgs::OrderedCollisionOperations	ord,
		const std::vector< motion_planning_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 869 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 = `.01`,
		double	angle_tol = `.09`,
		double	clip_dist = `.02`,
		double	clip_angle = `.16`,
		double	timestep = `0.1`
	)

Translate the gripper by direction using the Cartesian controllers.

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

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

Just a convenience function.

Definition at line 302 of file mechanism_interface.h.