trajectory_unwrap.h
Go to the documentation of this file.00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, 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 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of Willow Garage nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * Author: Melonee Wise 00036 *********************************************************************/ 00037 00038 #ifndef TRAJECTORY_UNWRAP_H_ 00039 #define TRAJECTORY_UNWRAP_H_ 00040 00041 #include <angles/angles.h> 00042 #include <urdf/model.h> 00043 #include <urdf/joint.h> 00044 #include <trajectory_msgs/JointTrajectory.h> 00045 00046 namespace trajectory_unwrap 00047 { 00048 00049 void unwrap(urdf::Model &robot_model, const trajectory_msgs::JointTrajectory& traj_in, 00050 trajectory_msgs::JointTrajectory& traj_out) 00051 { 00052 //defualt result 00053 traj_out = traj_in; 00054 00055 int num_jnts = traj_in.joint_names.size(); 00056 int traj_pnts = traj_in.points.size(); 00057 boost::shared_ptr<const urdf::Joint> joint; 00058 00059 for(int i=0; i<num_jnts; i++) 00060 { 00061 joint = robot_model.getJoint(traj_in.joint_names[i]); 00062 if(joint->type == urdf::Joint::REVOLUTE) 00063 { 00064 for(int j=1; j<traj_pnts; j++) 00065 { 00066 double diff; 00067 00068 angles::shortest_angular_distance_with_limits(traj_out.points[j-1].positions[i], traj_out.points[j].positions[i], joint->limits->lower, joint->limits->upper, diff); 00069 traj_out.points[j].positions[i] = traj_out.points[j-1].positions[i] + diff; 00070 } 00071 } 00072 if(joint->type == urdf::Joint::CONTINUOUS) 00073 { 00074 for(int j=1; j<traj_pnts; j++) 00075 { 00076 traj_out.points[j].positions[i] = traj_out.points[j-1].positions[i] + angles::shortest_angular_distance(traj_out.points[j-1].positions[i],traj_out.points[j].positions[i]); 00077 } 00078 } 00079 } 00080 } 00081 } 00082 #endif