$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2009, Willow Garage, Inc. 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 the Willow Garage 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 00037 #ifndef UNNORMALIZE_JOINT_TRAJECTORY_H_ 00038 #define UNNORMALIZE_JOINT_TRAJECTORY_H_ 00039 00040 #include <ros/ros.h> 00041 #include <urdf/model.h> 00042 #include <sensor_msgs/JointState.h> 00043 #include <spline_smoother/spline_smoother.h> 00044 #include <spline_smoother/spline_smoother_utils.h> 00045 00046 namespace joint_normalization_filters 00047 { 00048 00052 template <typename T> 00053 class UnNormalizeJointTrajectory: public spline_smoother::SplineSmoother<T> 00054 { 00055 private: 00057 ros::NodeHandle nh_; 00058 00060 urdf::Model robot_model_; 00061 00063 bool robot_model_initialized_; 00064 00065 public: 00066 UnNormalizeJointTrajectory(); 00067 virtual ~UnNormalizeJointTrajectory(); 00068 00069 virtual bool smooth(const T& trajectory_in, 00070 T& trajectory_out) const; 00071 }; 00072 00073 template <typename T> 00074 UnNormalizeJointTrajectory<T>::UnNormalizeJointTrajectory() 00075 { 00076 std::string urdf_xml,full_urdf_xml; 00077 nh_.param("urdf_xml", urdf_xml, std::string("robot_description")); 00078 if(!nh_.getParam(urdf_xml,full_urdf_xml)) 00079 { 00080 ROS_ERROR("Could not load the xml from parameter server: %s\n", urdf_xml.c_str()); 00081 robot_model_initialized_ = false; 00082 } 00083 else 00084 { 00085 robot_model_.initString(full_urdf_xml); 00086 robot_model_initialized_ = true; 00087 } 00088 } 00089 00090 template <typename T> 00091 UnNormalizeJointTrajectory<T>::~UnNormalizeJointTrajectory() 00092 { 00093 } 00094 00095 template <typename T> 00096 bool UnNormalizeJointTrajectory<T>::smooth(const T& trajectory_in, 00097 T& trajectory_out) const 00098 { 00099 trajectory_out = trajectory_in; 00100 00101 if (!spline_smoother::checkTrajectoryConsistency(trajectory_out)) 00102 return false; 00103 00104 if (!robot_model_initialized_) 00105 { 00106 ROS_ERROR("Robot model not initialized; can not tell continuous joints"); 00107 return false; 00108 } 00109 00110 if(trajectory_in.start_state.joint_state.name.size() == 0) { 00111 ROS_WARN_STREAM("Unnormalizer requires the start state to be set"); 00112 return false; 00113 } 00114 std::map<std::string, double> joint_values; 00115 for(unsigned int i = 0; i < trajectory_in.start_state.joint_state.name.size(); i++) { 00116 joint_values[trajectory_in.start_state.joint_state.name[i]] = trajectory_in.start_state.joint_state.position[i]; 00117 } 00118 00119 std::vector<double> current_values; 00120 std::vector<int> wraparound; 00121 trajectory_msgs::JointTrajectory input_trajectory = trajectory_in.trajectory; 00122 for (size_t i=0; i<input_trajectory.joint_names.size(); i++) 00123 { 00124 std::string name = input_trajectory.joint_names[i]; 00125 if(joint_values.find(name) == joint_values.end()) { 00126 ROS_WARN_STREAM("No value set in start state for joint name " << name); 00127 return false; 00128 } 00129 //first waypoint is unnormalized relative to current joint states 00130 current_values.push_back(joint_values[name]); 00131 00132 boost::shared_ptr<const urdf::Joint> joint = robot_model_.getJoint(name); 00133 if (joint.get() == NULL) 00134 { 00135 ROS_ERROR("Joint name %s not found in urdf model", name.c_str()); 00136 return false; 00137 } 00138 if (joint->type == urdf::Joint::CONTINUOUS) wraparound.push_back(1); 00139 else wraparound.push_back(0); 00140 } 00141 00142 trajectory_msgs::JointTrajectory normalized_trajectory = input_trajectory; 00143 for (size_t i=0; i<normalized_trajectory.points.size(); i++) 00144 { 00145 for (size_t j=0; j<normalized_trajectory.points[i].positions.size(); j++ ) 00146 { 00147 if (!wraparound.at(j)) continue; 00148 double current = current_values.at(j); 00149 double traj = normalized_trajectory.points[i].positions[j]; 00150 while ( current - traj > M_PI ) traj += 2*M_PI; 00151 while ( traj - current > M_PI ) traj -= 2*M_PI; 00152 ROS_DEBUG("Normalizing joint %s from %f to %f", normalized_trajectory.joint_names.at(j).c_str(), 00153 normalized_trajectory.points[i].positions[j], traj); 00154 normalized_trajectory.points[i].positions[j] = traj; 00155 //all other waypoints are unnormalized relative to the previous waypoint 00156 current_values.at(j) = traj; 00157 } 00158 } 00159 trajectory_out.trajectory = normalized_trajectory; 00160 return true; 00161 } 00162 } 00163 00164 #endif /* UNNORMALIZE_JOINT_TRAJECTORY_H_ */