#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.
RobotStatePtr	calculateRobotState (const RobotStatePtr &sourceRobotState, const SimpleVector3 &position, const SimpleQuaternion &orientation)
	Calculates the inverse kinematics for the given camera pose.
	MILDRobotModelWithExactIK ()
	constructor of the MILDRobotModelWithExactIK
void	setViewPointDistance (float viewPointDistance)
	Sets the distance between the desired view center point and the camera.
virtual	~MILDRobotModelWithExactIK ()
	destructor of the MILDRobotModelWithExactIK
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

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]`