Adapts the tracked end points of the operator to the robot's body proportions. More...

#include <motion_adaption.h>

Public Member Functions
void	adapt ()
	MotionAdaption ()
	~MotionAdaption ()
Private Member Functions
bool	adaptElbows ()
bool	adaptHands ()
bool	adaptHead ()
bool	adaptShoulders ()
bool	adaptTorso ()
bool	adaptTransforms ()
bool	getTransforms ()
void	publishData ()
bool	scaleUserHandsAndElbows ()
void	setGoals ()
bool	setRefFrame ()
Private Attributes
ros::Time	calc_time
double	elbow_x_
double	elbow_y_
tf::Vector3	head_goal_rot_vec_
tf::Transformer	internal_tf
bool	l_elbow_extended_
tf::Vector3	l_elbow_goal_rot_vec_
tf::Vector3	l_hand_goal_rot_vec_
double	limb_length_
tf::Matrix3x3	mat_orientation_
ros::NodeHandle	nh_
ros::NodeHandle	nh_private_
geometry_msgs::PoseStamped	pose_
ros::Publisher	pub_head_pose_
ros::Publisher	pub_l_elbow_pose_
ros::Publisher	pub_l_hand_pose_
ros::Publisher	pub_r_elbow_pose_
ros::Publisher	pub_r_hand_pose_
ros::Publisher	pub_torso_pose_
tf::Quaternion	quat_
tf::Quaternion	quat_adjust_
bool	r_elbow_extended_
tf::Vector3	r_elbow_goal_rot_vec_
tf::Vector3	r_hand_goal_rot_vec_
tf::Vector3	ref_frame_rot_vec_
double	robot_arm_length_
std::string	robot_base_str_
double	robot_head_height_
std::string	robot_head_str_
std::string	robot_l_elbow_str_
std::string	robot_l_hand_str_
std::string	robot_l_shoulder_str_
double	robot_lower_arm_length_
std::string	robot_r_elbow_str_
std::string	robot_r_hand_str_
std::string	robot_r_shoulder_str_
std::string	robot_ref_torso_str_
double	robot_shoulder_heigth_
double	robot_shoulder_width_
double	robot_shoulder_x_offset_
std::string	robot_torso_str_
double	robot_upper_arm_length_
tf::TransformBroadcaster	tf_broadcaster_
tf::StampedTransform	tf_head_goal_
tf::StampedTransform	tf_l_elbow_goal_
tf::StampedTransform	tf_l_elbow_hand_
tf::StampedTransform	tf_l_elbow_orient_
tf::StampedTransform	tf_l_elbow_pos_
tf::StampedTransform	tf_l_elbow_scaled_
tf::StampedTransform	tf_l_hand_goal_
tf::StampedTransform	tf_l_hand_scaled_
tf::StampedTransform	tf_l_shoulder_elbow_
tf::StampedTransform	tf_l_shoulder_goal_
tf::StampedTransform	tf_l_shoulder_hand_
tf::StampedTransform	tf_l_shoulder_scaled_
tf::TransformListener	tf_listener_
tf::StampedTransform	tf_r_elbow_goal_
tf::StampedTransform	tf_r_elbow_hand_
tf::StampedTransform	tf_r_elbow_orient_
tf::StampedTransform	tf_r_elbow_pos_
tf::StampedTransform	tf_r_elbow_scaled_
tf::StampedTransform	tf_r_hand_adjusted_
tf::StampedTransform	tf_r_hand_goal_
tf::StampedTransform	tf_r_hand_scaled_
tf::StampedTransform	tf_r_shoulder_elbow_
tf::StampedTransform	tf_r_shoulder_goal_
tf::StampedTransform	tf_r_shoulder_hand_
tf::StampedTransform	tf_r_shoulder_scaled_
tf::StampedTransform	tf_ref_frame_
tf::StampedTransform	tf_robot_r_elbow_r_hand_
tf::StampedTransform	tf_robot_r_shoulder_l_shoulder_
tf::StampedTransform	tf_robot_r_shoulder_r_elbow_
tf::StampedTransform	tf_robot_ref_torso_
tf::StampedTransform	tf_robot_torso_head_
tf::StampedTransform	tf_robot_torso_l_shoulder_
tf::StampedTransform	tf_robot_torso_r_shoulder_
tf::StampedTransform	tf_robot_torso_torso_
tf::StampedTransform	tf_torso_aligned_
tf::StampedTransform	tf_torso_goal_
tf::StampedTransform	tf_usr_head_
tf::StampedTransform	tf_usr_l_elbow_
tf::StampedTransform	tf_usr_l_elbow_hand_
tf::StampedTransform	tf_usr_l_hand_
tf::StampedTransform	tf_usr_l_shoulder_
tf::StampedTransform	tf_usr_l_shoulder_elbow_
tf::StampedTransform	tf_usr_l_shoulder_hand_
tf::StampedTransform	tf_usr_r_elbow_
tf::StampedTransform	tf_usr_r_elbow_hand_
tf::StampedTransform	tf_usr_r_hand_
tf::StampedTransform	tf_usr_r_shoulder_
tf::StampedTransform	tf_usr_r_shoulder_elbow_
tf::StampedTransform	tf_usr_r_shoulder_hand_
tf::StampedTransform	tf_usr_torso_
tf::Vector3	torso_goal_rot_vec_
double	user_arm_length_
std::string	user_head_str_
std::string	user_l_elbow_str_
std::string	user_l_hand_str_
std::string	user_l_shoulder_str_
std::string	user_r_elbow_str_
std::string	user_r_hand_str_
std::string	user_r_shoulder_str_
double	user_shoulder_height_
double	user_shoulder_width_
std::string	user_torso_str_
double	user_upper_arm_length_
tf::Vector3	vec_elbow_hand_
tf::Vector3	vec_helper_
tf::Vector3	vec_l_elbow_hand_valid_
tf::Vector3	vec_l_shoulder_elbow_valid_
tf::Vector3	vec_normal_
tf::Vector3	vec_r_elbow_hand_valid_
tf::Vector3	vec_r_shoulder_elbow_valid_
tf::Vector3	vec_shoulder_elbow_
tf::Vector3	vec_shoulder_hand_
double	wait_for_tf_
std::string	world_ref_frame_str_
double	x_adapt_
double	x_norm_
double	y_adapt_
double	y_norm_
double	z_adapt_
double	z_norm_

Detailed Description

Adapts the tracked end points of the operator to the robot's body proportions.

Author:: Marcus Liebhardt The adaptions uses the transforms of the specified operator end points, scales them according to the robot's body proportions and maps them onto the robot. The orientation of each end point is mapped unchanged.

Definition at line 52 of file motion_adaption.h.

Constructor & Destructor Documentation

MotionAdaption::MotionAdaption ( )

Definition at line 57 of file motion_adaption.cpp.

MotionAdaption::~MotionAdaption ( )

Definition at line 172 of file motion_adaption.cpp.

Member Function Documentation

void MotionAdaption::adapt ( )

This method consists of a hierarchy of all private methods of this class. Each method gets called, if the previous method has finished successfully.

Definition at line 182 of file motion_adaption.cpp.

bool MotionAdaption::adaptElbows ( ) [private]

The elbows are placed inside the arm planes at the intersection of the circles around the hands and elbows. Their radii are given by the upper and lower arm lengths. If arms are not completely stretched the normal vectors of the arm planes are used to chose among the two possible solutions. Further additions are made to handle small calculation errors, such as assuming stretched arms, when they are very close to stretched.

Returns:: true, if all necessary transforms have been retrieved successfully

Definition at line 310 of file adapt_transforms.cpp.

bool MotionAdaption::adaptHands ( ) [private]

The hands are placed at their scaled positions. The orientation is added manually in the way that the hands are aligned along the elbow. The plane normals are also used to define their orientation.

Returns:: true, if all necessary transforms have been retrieved successfully

Definition at line 405 of file adapt_transforms.cpp.

bool MotionAdaption::adaptHead ( ) [private]

Simple adaption of the operator's head transformation, which only applies the orientation of the operator's torso to the goal transformation; the position is a constant offset from the reference frame

Returns:: true, if all necessary transforms have been retrieved successfully

Definition at line 88 of file adapt_transforms.cpp.

bool MotionAdaption::adaptShoulders ( ) [private]

A constant offset from the reference frame defines the position of the shoulders. Their orientation is calculated based on the position of the elbows and hands. Therefore arm planes are calculated for both arms, which contain the elbows and hands.

Returns:: true, if all necessary transforms have been retrieved successfully

Definition at line 177 of file adapt_transforms.cpp.

bool MotionAdaption::adaptTorso ( ) [private]

Simple adaption of the operator's torso transformation, which only applies the orientation of the operator's torso to the goal transformation; the position is the same as the reference frame

returns always true

Definition at line 76 of file adapt_transforms.cpp.

bool MotionAdaption::adaptTransforms ( ) [private]

Contains the hierarchy of methods for adapting the several end points of the upper body.

Returns:: true, if all adaptions of the end points returned true

Author:: Marcus Liebhardt

Definition at line 42 of file adapt_transforms.cpp.

bool MotionAdaption::getTransforms ( ) [private]

Retrieves transforms from the operator's torso to the upper body end points hands, elbows, shoulder, head and the transform from the torso's reference frame to the torso.

Returns:: true, if all transforms of the operator have been retrieved successfully

Author:: Marcus Liebhardt

Definition at line 41 of file get_transforms.cpp.

void MotionAdaption::publishData ( ) [private]

Deprecated! Publishes the goal frames as messages. Not used anymore.

Author:: Marcus Liebhardt

Definition at line 41 of file publish_msgs.cpp.

bool MotionAdaption::scaleUserHandsAndElbows ( ) [private]

First part of the more complicated adaptions of the hands: This method scales the position of the hands and elbows using both the body proportions of the operator and the robot.