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


pr2_arm_move_ik
Author(s): Melonee Wise, Wim Meeussen
autogenerated on Tue Apr 22 2014 19:29:55