motion_adaption.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (Modified BSD License)
00003  *
00004  *  Copyright (c) 2011, PAL Robotics, S.L.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of PAL Robotics, S.L. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00038 #include "motion_adaption/motion_adaption.h"
00039 
00040 
00041 static const double WAIT_FOR_TF_IN_S = 0.5;
00042 static const std::string NO_NAME_STRING = "no valid transform name";
00043 static const double ROBOT_SHOULDER_HEIGHT_DEFAULT = 0.21732;
00044 static const double ROBOT_SHOULDER_WIDTH_DEFAULT = 0.46382;
00045 static const double ROBOT_SHOULDER_X_OFFSET_DEFAULT = 0.01500;
00046 static const double ROBOT_UPPER_ARM_LENGTH_DEFAULT = 0.23000;
00047 static const double ROBOT_LOWER_ARM_LENGTH_DEFAULT = 0.23950;
00048 static const double ROBOT_HEAD_HEIGHT_DEFAULT = 0.41200;
00049 static const std::string SUB_TOPIC_POSE_TORSO = "pose_torso";
00050 static const std::string SUB_TOPIC_POSE_HEAD = "pose_head";
00051 static const std::string SUB_TOPIC_POSE_ELBOW_RIGHT = "pose_elbow_right";
00052 static const std::string SUB_TOPIC_POSE_HAND_RIGHT = "pose_hand_right";
00053 static const std::string SUB_TOPIC_POSE_ELBOW_LEFT = "pose_elbow_left";
00054 static const std::string SUB_TOPIC_POSE_HAND_LEFT = "pose_hand_left";
00055 
00056 
00057 MotionAdaption::MotionAdaption(): nh_private_("~")
00058 {
00059   nh_private_.param("wait_for_tf_in_sec", wait_for_tf_, WAIT_FOR_TF_IN_S);
00060   nh_private_.param("world_ref_frame_name", world_ref_frame_str_, NO_NAME_STRING);
00061   nh_private_.param("user/torso_frame_name", user_torso_str_, NO_NAME_STRING);
00062   nh_private_.param("user/head_frame_name", user_head_str_, NO_NAME_STRING);
00063   nh_private_.param("user/right_shoulder_frame_name", user_r_shoulder_str_, NO_NAME_STRING);
00064   nh_private_.param("user/right_elbow_frame_name", user_r_elbow_str_, NO_NAME_STRING);
00065   nh_private_.param("user/right_hand_frame_name", user_r_hand_str_, NO_NAME_STRING);
00066   nh_private_.param("user/left_shoulder_frame_name", user_l_shoulder_str_, NO_NAME_STRING);
00067   nh_private_.param("user/left_elbow_frame_name", user_l_elbow_str_, NO_NAME_STRING);
00068   nh_private_.param("user/left_hand_frame_name", user_l_hand_str_, NO_NAME_STRING);
00069   nh_private_.param("robot/shoulder_heigth", robot_shoulder_heigth_, ROBOT_SHOULDER_HEIGHT_DEFAULT);
00070   nh_private_.param("robot/shoulder_width", robot_shoulder_width_, ROBOT_SHOULDER_WIDTH_DEFAULT);
00071   nh_private_.param("robot/shoulder_x_offset", robot_shoulder_x_offset_, ROBOT_SHOULDER_X_OFFSET_DEFAULT);
00072   nh_private_.param("robot/upper_arm_length", robot_upper_arm_length_, ROBOT_UPPER_ARM_LENGTH_DEFAULT);
00073   nh_private_.param("robot/lower_arm_length", robot_lower_arm_length_, ROBOT_LOWER_ARM_LENGTH_DEFAULT);
00074   nh_private_.param("robot/head_height", robot_head_height_, ROBOT_HEAD_HEIGHT_DEFAULT);
00075   robot_arm_length_ = robot_upper_arm_length_ + robot_lower_arm_length_;
00076   nh_private_.param("robot/base_frame_name", robot_base_str_, NO_NAME_STRING);
00077   //nh_private_.param("robot/torso_frame_name", robot_torso_str_, NO_NAME_STRING);  
00078   nh_private_.param("robot/torso_ref_frame_name", robot_ref_torso_str_, NO_NAME_STRING);
00079   nh_private_.param("robot/head_frame_name", robot_head_str_, NO_NAME_STRING);
00080   nh_private_.param("robot/right_shoulder_frame_name", robot_r_shoulder_str_, NO_NAME_STRING);      
00081   nh_private_.param("robot/right_elbow_frame_name", robot_r_elbow_str_, NO_NAME_STRING); 
00082   nh_private_.param("robot/right_hand_frame_name", robot_r_hand_str_, NO_NAME_STRING); 
00083   nh_private_.param("robot/left_shoulder_frame_name", robot_l_shoulder_str_, NO_NAME_STRING);      
00084   //nh_private_.param("robot/left_elbow_frame_name", robot_l_elbow_str_, NO_NAME_STRING); 
00085   //nh_private_.param("robot/left_hand_frame_name", robot_l_hand_str_, NO_NAME_STRING);    
00086   nh_private_.param("rotation_angles/ref_frame_rot/r", ref_frame_rot_vec_[0], 0.0);
00087   nh_private_.param("rotation_angles/ref_frame_rot/p", ref_frame_rot_vec_[1], 0.0);
00088   nh_private_.param("rotation_angles/ref_frame_rot/y", ref_frame_rot_vec_[2], 0.0);
00089   nh_private_.param("rotation_angles/goals/torso_goal_rot/r", torso_goal_rot_vec_[0], 0.0);
00090   nh_private_.param("rotation_angles/goals/torso_goal_rot/p", torso_goal_rot_vec_[1], 0.0);
00091   nh_private_.param("rotation_angles/goals/torso_goal_rot/y", torso_goal_rot_vec_[2], 0.0);
00092   nh_private_.param("rotation_angles/goals/head_goal_rot/r", head_goal_rot_vec_[0], 0.0);
00093   nh_private_.param("rotation_angles/goals/head_goal_rot/p", head_goal_rot_vec_[1], 0.0);
00094   nh_private_.param("rotation_angles/goals/head_goal_rot/y", head_goal_rot_vec_[2], 0.0);
00095   nh_private_.param("rotation_angles/goals/right_elbow_goal_rot/r", r_elbow_goal_rot_vec_[0], 0.0);
00096   nh_private_.param("rotation_angles/goals/right_elbow_goal_rot/p", r_elbow_goal_rot_vec_[1], 0.0);
00097   nh_private_.param("rotation_angles/goals/right_elbow_goal_rot/y", r_elbow_goal_rot_vec_[2], 0.0);
00098   nh_private_.param("rotation_angles/goals/right_hand_goal_rot/r", r_hand_goal_rot_vec_[0], 0.0);
00099   nh_private_.param("rotation_angles/goals/right_hand_goal_rot/p", r_hand_goal_rot_vec_[1], 0.0);
00100   nh_private_.param("rotation_angles/goals/right_hand_goal_rot/y", r_hand_goal_rot_vec_[2], 0.0);
00101   nh_private_.param("rotation_angles/goals/left_elbow_goal_rot/r", l_elbow_goal_rot_vec_[0], 0.0);
00102   nh_private_.param("rotation_angles/goals/left_elbow_goal_rot/p", l_elbow_goal_rot_vec_[1], 0.0);
00103   nh_private_.param("rotation_angles/goals/left_elbow_goal_rot/y", l_elbow_goal_rot_vec_[2], 0.0);
00104   nh_private_.param("rotation_angles/goals/left_hand_goal_rot/r", l_hand_goal_rot_vec_[0], 0.0);
00105   nh_private_.param("rotation_angles/goals/left_hand_goal_rot/p", l_hand_goal_rot_vec_[1], 0.0);
00106   nh_private_.param("rotation_angles/goals/left_hand_goal_rot/y", l_hand_goal_rot_vec_[2], 0.0);  
00107   pub_torso_pose_ = nh_private_.advertise<geometry_msgs::PoseStamped> (SUB_TOPIC_POSE_TORSO, 1);
00108   pub_head_pose_ = nh_private_.advertise<geometry_msgs::PoseStamped> (SUB_TOPIC_POSE_HEAD, 1);
00109   pub_r_elbow_pose_ = nh_private_.advertise<geometry_msgs::PoseStamped> (SUB_TOPIC_POSE_ELBOW_RIGHT, 1);
00110   pub_r_hand_pose_ = nh_private_.advertise<geometry_msgs::PoseStamped> (SUB_TOPIC_POSE_HAND_RIGHT, 1);
00111   pub_l_elbow_pose_ = nh_private_.advertise<geometry_msgs::PoseStamped> (SUB_TOPIC_POSE_ELBOW_LEFT, 1);
00112   pub_l_hand_pose_ = nh_private_.advertise<geometry_msgs::PoseStamped> (SUB_TOPIC_POSE_HAND_LEFT, 1);
00113 
00114   // initialize variables
00115   user_shoulder_width_ = user_shoulder_height_ = user_arm_length_ = 0.0;
00116   x_norm_ = y_norm_ = z_norm_ = 0.0;
00117   x_adapt_ = y_adapt_ = z_adapt_ = 0.0;
00118   elbow_x_ = elbow_y_ = 0.0;
00119   limb_length_ = 0.0;
00120 
00121   tf_ref_frame_.setIdentity();
00122   tf_torso_goal_.setIdentity();
00123   tf_head_goal_.setIdentity();
00124   tf_r_shoulder_elbow_.setIdentity();
00125   tf_r_shoulder_hand_.setIdentity();
00126   tf_r_shoulder_scaled_.setIdentity();
00127   tf_r_shoulder_goal_.setIdentity();
00128   tf_r_elbow_scaled_.setIdentity();
00129   tf_r_elbow_pos_.setIdentity();
00130   tf_l_elbow_orient_.setIdentity();
00131   tf_r_elbow_hand_.setIdentity();
00132   tf_r_elbow_goal_.setIdentity();
00133   tf_r_hand_scaled_.setIdentity();
00134   tf_r_hand_adjusted_.setIdentity();
00135   tf_r_hand_goal_.setIdentity();
00136   tf_l_shoulder_elbow_.setIdentity();
00137   tf_l_shoulder_hand_.setIdentity();
00138   tf_l_shoulder_scaled_.setIdentity();
00139   tf_l_shoulder_goal_.setIdentity();
00140   tf_l_elbow_scaled_.setIdentity();
00141   tf_l_elbow_pos_.setIdentity();
00142   tf_l_elbow_orient_.setIdentity();
00143   tf_l_elbow_hand_.setIdentity();
00144   tf_l_elbow_goal_.setIdentity();
00145   tf_l_hand_scaled_.setIdentity();
00146   tf_l_hand_goal_.setIdentity();
00147 
00148   tf_usr_r_shoulder_.setIdentity();
00149   tf_usr_r_shoulder_elbow_.setIdentity();
00150   tf_usr_r_elbow_hand_.setIdentity();
00151   tf_usr_r_hand_.setIdentity();
00152   tf_usr_l_shoulder_.setIdentity();
00153   tf_usr_l_shoulder_elbow_.setIdentity();
00154   tf_usr_l_elbow_hand_.setIdentity();
00155   tf_usr_l_hand_.setIdentity();
00156 
00157   tf_robot_ref_torso_.setIdentity();  
00158   tf_robot_torso_torso_.setIdentity();
00159   tf_robot_torso_head_.setIdentity();
00160   tf_robot_torso_r_shoulder_.setIdentity();
00161   tf_robot_r_shoulder_l_shoulder_.setIdentity();    
00162   tf_robot_r_shoulder_r_elbow_.setIdentity();
00163   tf_robot_r_elbow_r_hand_.setIdentity();  
00164   
00165   vec_r_elbow_hand_valid_.setZero();
00166   vec_l_elbow_hand_valid_.setZero();
00167 
00168   r_elbow_extended_ = false;
00169   l_elbow_extended_ = false;
00170 }
00171 
00172 MotionAdaption::~MotionAdaption()
00173 {
00174   pub_torso_pose_.shutdown();
00175   pub_head_pose_.shutdown();
00176   pub_r_elbow_pose_.shutdown();
00177   pub_r_hand_pose_.shutdown();
00178   pub_l_elbow_pose_.shutdown();
00179   pub_l_hand_pose_.shutdown();
00180 }
00181 
00182 void MotionAdaption::adapt()
00183 {  
00184   calc_time = ros::Time::now();
00185   if(getTransforms())
00186   {
00187     if(setRefFrame())
00188     {
00189       if(adaptTransforms())
00190       {
00191         setGoals();
00192         //publishData();
00193       }
00194     }
00195   }
00196 }
00197 


motion_adaption
Author(s): Marcus Liebhardt
autogenerated on Mon Jan 6 2014 11:39:55