Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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.request.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.request.start_state.joint_state.name.size(); i++) {
00116 joint_values[trajectory_in.request.start_state.joint_state.name[i]] = trajectory_in.request.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.request.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
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
00156 current_values.at(j) = traj;
00157 }
00158 }
00159 trajectory_out.request.trajectory = normalized_trajectory;
00160 return true;
00161 }
00162 }
00163
00164 #endif