#include <MILDRobotModelWithExactIK.hpp>

Inheritance diagram for robot_model_services::MILDRobotModelWithExactIK:

Public Member Functions
PTUConfig	calculateCameraPoseCorrection (const RobotStatePtr &sourceRobotState, const SimpleVector3 &position, const SimpleQuaternion &orientation)
	Calculates the best approximation for the given camera pose from the current base position. More...

RobotStatePtr	calculateRobotState (const RobotStatePtr &sourceRobotState, const SimpleVector3 &position, const SimpleQuaternion &orientation)
	Calculates the inverse kinematics for the given camera pose. More...

	MILDRobotModelWithExactIK ()
	constructor of the MILDRobotModelWithExactIK More...

void	setViewPointDistance (float viewPointDistance)
	Sets the distance between the desired view center point and the camera. More...

virtual	~MILDRobotModelWithExactIK ()
	destructor of the MILDRobotModelWithExactIK More...

Public Member Functions inherited from robot_model_services::MILDRobotModel
geometry_msgs::Pose	calculateCameraPose (const RobotStatePtr &sourceRobotState)
	Uses a given RobotState to calculate the camera frame. More...

float	getBase_RotationalMovementCosts (const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState)

float	getBase_TranslationalMovementCosts (const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState)
	Calculates the movement costs from sourceRobotState to targetRobotState. Returns -1 if pose is not reachable. More...

geometry_msgs::Pose	getCameraPose ()

float	getDistance (const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition)

nav_msgs::Path	getNavigationPath (const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition, double sourceRotationBase, double targetRotationBase)

nav_msgs::Path	getNavigationPath (const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition)

float	getPTU_PanMovementCosts (const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState)

float	getPTU_TiltMovementCosts (const RobotStatePtr &sourceRobotState, const RobotStatePtr &targetRobotState)

geometry_msgs::Pose	getRobotPose ()

bool	isPoseReachable (const SimpleVector3 &position, const SimpleQuaternion &orientation)

bool	isPositionAllowed (const geometry_msgs::Point &position)

bool	isPositionReachable (const geometry_msgs::Point &sourcePosition, const geometry_msgs::Point &targetPosition)

	MILDRobotModel ()
	constructor of the MILDRobotModel More...

void	setPanAngleLimits (float minAngleDegrees, float maxAngleDegrees)
	sets the angle limits of the pan angle. More...

void	setRotationAngleLimits (float minAngleDegrees, float maxAngleDegrees)
	sets the angle limits of the rotation angle. More...

void	setTiltAngleLimits (float minAngleDegrees, float maxAngleDegrees)
	sets the angle limits of the tilt angle. More...

virtual	~MILDRobotModel ()
	destructor of the MILDRobotModel More...

Public Member Functions inherited from robot_model_services::RobotModel
RobotStatePtr	calculateRobotState (const robot_model_services::SimpleVector3 &position, const robot_model_services::SimpleQuaternion &orientation)
	calculates the target robot state by assuming the saved current state of the roboter as source state More...

float	getBase_RotationalMovementCosts (const RobotStatePtr &targetRobotState)

float	getBase_TranslationalMovementCosts (const RobotStatePtr &targetRobotState)

RobotStatePtr	getCurrentRobotState ()

float	getPTU_PanMovementCosts (const RobotStatePtr &targetRobotState)

float	getPTU_TiltMovementCosts (const RobotStatePtr &targetRobotState)

	RobotModel ()
	the constructor of the RobotModel object More...

void	setCurrentRobotState (const RobotStatePtr &currentRobotState)

virtual	~RobotModel ()
	the destructor of the RobotModel object More...

Public Attributes
ros::Publisher	vis_pub

Private Member Functions
double	acosClamped (double value)

double	getBaseAngleFromBaseFrame (Eigen::Affine3d &baseFrame)

double	getPanAngleFromPanJointPose (Eigen::Affine3d &panJointFrame, MILDRobotStatePtr &robotState)

bool	getTiltAngleAndTiltBasePointProjected (Eigen::Vector3d &planeNormal, Eigen::Vector3d &targetViewVector, Eigen::Vector3d &target_view_center_point, double &tilt, Eigen::Vector3d &tilt_base_point_projected)

Eigen::Affine3d	getTiltJointFrame (Eigen::Vector3d &planeNormal, Eigen::Vector3d &targetViewVector, Eigen::Vector3d &tilt_base_point)

void	resetIKVisualization ()

bool	setUpTFParameters ()

void	visualizeCameraPoseCorrection (Eigen::Vector3d &base_point, Eigen::Vector3d &base_orientation, Eigen::Vector3d &pan_joint_point, Eigen::Vector3d &pan_rotated_point, Eigen::Vector3d &tilt_base_point, Eigen::Vector3d &cam_point, Eigen::Vector3d &actual_view_center_point)

void	visualizeIKArrow (Eigen::Vector3d &pointStart, Eigen::Vector3d &pointEnd, Eigen::Vector4d &colorRGBA, std::string ns, Eigen::Vector3d &scaleParameters, int id)

void	visualizeIKArrowLarge (Eigen::Vector3d &pointStart, Eigen::Vector3d &pointEnd, Eigen::Vector4d &colorRGBA, std::string ns, int id)

void	visualizeIKArrowSmall (Eigen::Vector3d &pointStart, Eigen::Vector3d &pointEnd, Eigen::Vector4d &colorRGBA, std::string ns, int id)

void	visualizeIKcalculation (Eigen::Vector3d &base_point, Eigen::Vector3d &base_orientation, Eigen::Vector3d &pan_joint_point, Eigen::Vector3d &pan_rotated_point, Eigen::Vector3d &tilt_base_point, Eigen::Vector3d &tilt_base_point_projected, Eigen::Vector3d &cam_point, Eigen::Vector3d &actual_view_center_point)

void	visualizeIKCameraTarget (Eigen::Vector3d &target_view_center_point, Eigen::Vector3d &target_camera_point)

void	visualizeIKPoint (Eigen::Vector3d &point, Eigen::Vector4d &colorRGBA, std::string ns, int id)

Private Attributes
Eigen::Affine3d	baseToPanEigen

double	h_tilt

IKRatingModulePtr	ikRatingModule

const unsigned int	IKVisualizationMaximunIterationCount = 6

unsigned int	mIKVisualizationLastIterationCount = 0

float	mInverseKinematicIterationAccuracy

double	mnTotalIKTime

int	mNumberIKCalls

double	mPanAngleOffset

unsigned int	mPanAngleSamplingStepsPerIteration

double	mTiltAngleOffset

double	mViewPointDistance

bool	mVisualizeIK

Eigen::Affine3d	panToBaseEigen

Eigen::Affine3d	panToTiltEigen

bool	tfParametersInitialized

Eigen::Affine3d	tiltToCameraEigen

Eigen::Affine3d	tiltToPanEigen

double	viewTriangleXYPlane_sideC

double	viewTriangleZPlane_angleAlpha

double	viewTriangleZPlane_angleGamma

double	viewTriangleZPlane_sideA

double	viewTriangleZPlane_sideB

double	x_product

Additional Inherited Members
Protected Attributes inherited from robot_model_services::MILDRobotModel
tf::TransformListener	listener

DebugHelperPtr	mDebugHelperPtr

std::string	mFrameName_map

std::string	mFrameName_mild_base

std::string	mFrameName_mild_camera_left

std::string	mFrameName_mild_camera_mount_link

std::string	mFrameName_mild_camera_right

std::string	mFrameName_mild_ptu_base_link

std::string	mFrameName_mild_ptu_pan_link

std::string	mFrameName_mild_ptu_pan_link_rotated

std::string	mFrameName_mild_ptu_tilt_link

std::string	mFrameName_mild_ptu_tilt_link_rotated

MapHelperPtr	mMapHelperPtr

float	mOmegaPan

float	mOmegaRot

float	mOmegaTilt

boost::tuple< float, float >	mPanLimits

boost::tuple< float, float >	mRotationLimits

float	mSigma

boost::tuple< float, float >	mTiltLimits

ros::NodeHandle	n

ros::ServiceClient	navigationCostClient

float	speedFactorBaseMove

float	speedFactorBaseRot

float	speedFactorPTU

float	tolerance

bool	useGlobalPlanner

Detailed Description

Definition at line 41 of file MILDRobotModelWithExactIK.hpp.

Constructor & Destructor Documentation

robot_model_services::MILDRobotModelWithExactIK::MILDRobotModelWithExactIK ( )

constructor of the MILDRobotModelWithExactIK

Definition at line 54 of file MILDRobotModelWithExactIK.cpp.

robot_model_services::MILDRobotModelWithExactIK::~MILDRobotModelWithExactIK ( )

virtual

destructor of the MILDRobotModelWithExactIK

Definition at line 116 of file MILDRobotModelWithExactIK.cpp.

Member Function Documentation

double robot_model_services::MILDRobotModelWithExactIK::acosClamped ( double value )

private

Helper: Calculates the acos of the value. If value is not within [-1, 1], clamps the value accordingly

PTUConfig robot_model_services::MILDRobotModelWithExactIK::calculateCameraPoseCorrection	(	const RobotStatePtr &	sourceRobotState,
		const SimpleVector3 &	position,
		const SimpleQuaternion &	orientation
	)

Calculates the best approximation for the given camera pose from the current base position.

Definition at line 125 of file MILDRobotModelWithExactIK.cpp.

RobotStatePtr robot_model_services::MILDRobotModelWithExactIK::calculateRobotState	(	const RobotStatePtr &	sourceRobotState,
		const SimpleVector3 &	position,
		const SimpleQuaternion &	orientation
	)

virtual

Calculates the inverse kinematics for the given camera pose.

Implements robot_model_services::RobotModel.

Definition at line 259 of file MILDRobotModelWithExactIK.cpp.

double robot_model_services::MILDRobotModelWithExactIK::getBaseAngleFromBaseFrame ( Eigen::Affine3d & baseFrame )

private

Calculates the rotation angle of the base from its pose frame

Definition at line 869 of file MILDRobotModelWithExactIK.cpp.

double robot_model_services::MILDRobotModelWithExactIK::getPanAngleFromPanJointPose	(	Eigen::Affine3d &	panJointFrame,
		MILDRobotStatePtr &	robotState
	)

private

Calculates the optimal pan angle for the given pose of the pan joint

Definition at line 504 of file MILDRobotModelWithExactIK.cpp.

bool robot_model_services::MILDRobotModelWithExactIK::getTiltAngleAndTiltBasePointProjected	(	Eigen::Vector3d &	planeNormal,
		Eigen::Vector3d &	targetViewVector,
		Eigen::Vector3d &	target_view_center_point,
		double &	tilt,
		Eigen::Vector3d &	tilt_base_point_projected
	)

private

Calculates the tilt angle and the projected pose of the tilt joint returns true if successful

Definition at line 430 of file MILDRobotModelWithExactIK.cpp.

Eigen::Affine3d robot_model_services::MILDRobotModelWithExactIK::getTiltJointFrame	(	Eigen::Vector3d &	planeNormal,
		Eigen::Vector3d &	targetViewVector,
		Eigen::Vector3d &	tilt_base_point
	)

private

Calculates the transformation frame of the tilt joint

Definition at line 414 of file MILDRobotModelWithExactIK.cpp.

void robot_model_services::MILDRobotModelWithExactIK::resetIKVisualization ( )

private

Deletes all visualization markers for the IK calculation

Definition at line 671 of file MILDRobotModelWithExactIK.cpp.

bool robot_model_services::MILDRobotModelWithExactIK::setUpTFParameters ( )

private

Trys to calculate parameters needed for the inverse kinematic using tf transformations

Definition at line 579 of file MILDRobotModelWithExactIK.cpp.

void robot_model_services::MILDRobotModelWithExactIK::setViewPointDistance ( float viewPointDistance )

Sets the distance between the desired view center point and the camera.

Definition at line 118 of file MILDRobotModelWithExactIK.cpp.

void robot_model_services::MILDRobotModelWithExactIK::visualizeCameraPoseCorrection	(	Eigen::Vector3d &	base_point,
		Eigen::Vector3d &	base_orientation,
		Eigen::Vector3d &	pan_joint_point,
		Eigen::Vector3d &	pan_rotated_point,
		Eigen::Vector3d &	tilt_base_point,
		Eigen::Vector3d &	cam_point,
		Eigen::Vector3d &	actual_view_center_point
	)

private

Visualizes the output of the CameraPoseCorrection

Definition at line 788 of file MILDRobotModelWithExactIK.cpp.

void robot_model_services::MILDRobotModelWithExactIK::visualizeIKArrow	(	Eigen::Vector3d &	pointStart,
		Eigen::Vector3d &	pointEnd,
		Eigen::Vector4d &	colorRGBA,
		std::string	ns,
		Eigen::Vector3d &	scaleParameters,
		int	id
	)

private

Visualizes the translation between two frames through an arrow

Definition at line 839 of file MILDRobotModelWithExactIK.cpp.

void robot_model_services::MILDRobotModelWithExactIK::visualizeIKArrowLarge	(	Eigen::Vector3d &	pointStart,
		Eigen::Vector3d &	pointEnd,
		Eigen::Vector4d &	colorRGBA,
		std::string	ns,
		int	id
	)

private

Visualizes the translation between two frames through an arrow

Definition at line 833 of file MILDRobotModelWithExactIK.cpp.

void robot_model_services::MILDRobotModelWithExactIK::visualizeIKArrowSmall	(	Eigen::Vector3d &	pointStart,
		Eigen::Vector3d &	pointEnd,
		Eigen::Vector4d &	colorRGBA,
		std::string	ns,
		int	id
	)

private

Visualizes the translation between two frames through an arrow

Definition at line 827 of file MILDRobotModelWithExactIK.cpp.

void robot_model_services::MILDRobotModelWithExactIK::visualizeIKcalculation	(	Eigen::Vector3d &	base_point,
		Eigen::Vector3d &	base_orientation,
		Eigen::Vector3d &	pan_joint_point,
		Eigen::Vector3d &	pan_rotated_point,
		Eigen::Vector3d &	tilt_base_point,
		Eigen::Vector3d &	tilt_base_point_projected,
		Eigen::Vector3d &	cam_point,
		Eigen::Vector3d &	actual_view_center_point
	)

private

Visualizes the output of the IK calculation

Definition at line 771 of file MILDRobotModelWithExactIK.cpp.

void robot_model_services::MILDRobotModelWithExactIK::visualizeIKCameraTarget	(	Eigen::Vector3d &	target_view_center_point,
		Eigen::Vector3d &	target_camera_point
	)

private

Visualizes the IK camera target

Definition at line 765 of file MILDRobotModelWithExactIK.cpp.

void robot_model_services::MILDRobotModelWithExactIK::visualizeIKPoint	(	Eigen::Vector3d &	point,
		Eigen::Vector4d &	colorRGBA,
		std::string	ns,
		int	id
	)

private

Visualizes a point for a single frame position

Definition at line 803 of file MILDRobotModelWithExactIK.cpp.

Member Data Documentation

Eigen::Affine3d robot_model_services::MILDRobotModelWithExactIK::baseToPanEigen

private

Transformation frame from the base-link to the pan-link

Definition at line 88 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::h_tilt

private

Height of the tilt axis above ground

Definition at line 57 of file MILDRobotModelWithExactIK.hpp.

IKRatingModulePtr robot_model_services::MILDRobotModelWithExactIK::ikRatingModule

private

The rating module for the inverse kinematic sampling

Definition at line 52 of file MILDRobotModelWithExactIK.hpp.

const unsigned int robot_model_services::MILDRobotModelWithExactIK::IKVisualizationMaximunIterationCount = 6

private

Definition at line 45 of file MILDRobotModelWithExactIK.hpp.

unsigned int robot_model_services::MILDRobotModelWithExactIK::mIKVisualizationLastIterationCount = 0

private

Definition at line 64 of file MILDRobotModelWithExactIK.hpp.

float robot_model_services::MILDRobotModelWithExactIK::mInverseKinematicIterationAccuracy

private

Definition at line 47 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::mnTotalIKTime

private

Definition at line 100 of file MILDRobotModelWithExactIK.hpp.

int robot_model_services::MILDRobotModelWithExactIK::mNumberIKCalls

private

Definition at line 99 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::mPanAngleOffset

private

Definition at line 66 of file MILDRobotModelWithExactIK.hpp.

unsigned int robot_model_services::MILDRobotModelWithExactIK::mPanAngleSamplingStepsPerIteration

private

Definition at line 63 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::mTiltAngleOffset

private

Definition at line 65 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::mViewPointDistance

private

Definition at line 67 of file MILDRobotModelWithExactIK.hpp.

bool robot_model_services::MILDRobotModelWithExactIK::mVisualizeIK

private

Detemines if the IK calculation should be visualized by markers. Turn this off to speed up calculation

Definition at line 72 of file MILDRobotModelWithExactIK.hpp.

Eigen::Affine3d robot_model_services::MILDRobotModelWithExactIK::panToBaseEigen

private

Definition at line 89 of file MILDRobotModelWithExactIK.hpp.

Eigen::Affine3d robot_model_services::MILDRobotModelWithExactIK::panToTiltEigen

private

Transformation frame from the pan-link to the tilt-link

Definition at line 82 of file MILDRobotModelWithExactIK.hpp.

bool robot_model_services::MILDRobotModelWithExactIK::tfParametersInitialized

private

Flag, shows if the tf parameters have already been initialized

Definition at line 96 of file MILDRobotModelWithExactIK.hpp.

Eigen::Affine3d robot_model_services::MILDRobotModelWithExactIK::tiltToCameraEigen

private

Transformation frame from the tilted-link to camera left

Definition at line 77 of file MILDRobotModelWithExactIK.hpp.

Eigen::Affine3d robot_model_services::MILDRobotModelWithExactIK::tiltToPanEigen

private

Definition at line 83 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::viewTriangleXYPlane_sideC

private

Definition at line 62 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::viewTriangleZPlane_angleAlpha

private

Definition at line 58 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::viewTriangleZPlane_angleGamma

private

Definition at line 59 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::viewTriangleZPlane_sideA

private

Definition at line 60 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::viewTriangleZPlane_sideB

private

Definition at line 61 of file MILDRobotModelWithExactIK.hpp.

ros::Publisher robot_model_services::MILDRobotModelWithExactIK::vis_pub

Definition at line 188 of file MILDRobotModelWithExactIK.hpp.

double robot_model_services::MILDRobotModelWithExactIK::x_product

private

Definition at line 91 of file MILDRobotModelWithExactIK.hpp.

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

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation