00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2012, 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 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 00035 /* Author: Ioan Sucan */ 00036 00037 #include <moveit/planning_request_adapter/planning_request_adapter.h> 00038 #include <moveit/trajectory_processing/iterative_time_parameterization.h> 00039 #include <class_loader/class_loader.h> 00040 #include <ros/console.h> 00041 00042 /* 00043 #include <ReflexxesAPI.h> 00044 #include <RMLPositionFlags.h> 00045 #include <RMLPositionInputParameters.h> 00046 #include <RMLPositionOutputParameters.h> 00047 */ 00048 00049 namespace default_planner_request_adapters 00050 { 00051 00052 class AddTimeParameterization : public planning_request_adapter::PlanningRequestAdapter 00053 { 00054 public: 00055 00056 AddTimeParameterization() : planning_request_adapter::PlanningRequestAdapter() 00057 { 00058 } 00059 /* 00060 void reflexxesComputeTime(robot_trajectory::RobotTrajectory &traj) const 00061 { 00062 00063 boost::scoped_ptr<ReflexxesAPI> RML; 00064 boost::scoped_ptr<RMLPositionInputParameters> IP; 00065 boost::scoped_ptr<RMLPositionOutputParameters> OP; 00066 RMLPositionFlags Flags; 00067 00068 // ******************************************************************** 00069 // Creating all relevant objects of the Reflexxes Motion Library 00070 00071 RML.reset(new ReflexxesAPI(7, 0.01)); 00072 IP.reset(new RMLPositionInputParameters(7)); 00073 OP.reset(new RMLPositionOutputParameters(7)); 00074 00075 std::vector<double> v; 00076 traj.getFirstWayPoint().getJointStateGroup(traj.getGroup()->getName())->getVariableValues(v); 00077 00078 for (int i = 0; i < 7 ; ++i) 00079 { 00080 IP->CurrentPositionVector->VecData [i] = v[i]; // rad 00081 IP->CurrentVelocityVector->VecData [i] = 0.0 ; // rad/s 00082 IP->CurrentAccelerationVector->VecData [i] = 0.0 ; // not needed for type 2 00083 IP->MaxVelocityVector->VecData [i] = 1.5 ; // rad/s 00084 IP->MaxAccelerationVector->VecData [i] = 1.0 ; // 00085 IP->MaxJerkVector->VecData [i] = 0.1 ; // not needed for type 2 00086 IP->TargetPositionVector->VecData [i] = v[i]; // first waypoint 00087 IP->TargetVelocityVector->VecData [i] = 0.0 ; // velocity at first waypoint 00088 IP->SelectionVector->VecData [i] = true ; 00089 } 00090 bool error = false; 00091 for (std::size_t i = 1 ; !error && i < traj.getWayPointCount() ; ++i) 00092 { 00093 int ResultValue = 0; 00094 double totalt = 0.0; 00095 00096 while (ResultValue != ReflexxesAPI::RML_FINAL_STATE_REACHED) 00097 { 00098 00099 // **************************************************************** 00100 // Wait for the next timer tick 00101 // (not implemented in this example in order to keep it simple) 00102 // **************************************************************** 00103 00104 // Calling the Reflexxes OTG algorithm 00105 ResultValue = RML->RMLPosition(*IP, OP.get(), Flags); 00106 00107 if (ResultValue < 0) 00108 { 00109 printf("An error occurred (%d).\n", ResultValue ); 00110 error = true; 00111 break; 00112 } 00113 00114 totalt += 0.01; 00115 00116 00117 // **************************************************************** 00118 // Here, the new state of motion, that is 00119 // 00120 // - OP->NewPositionVector 00121 // - OP->NewVelocityVector 00122 // - OP->NewAccelerationVector 00123 // 00124 // can be used as input values for lower level controllers. In the 00125 // most simple case, a position controller in actuator space is 00126 // used, but the computed state can be applied to many other 00127 // controllers (e.g., Cartesian impedance controllers, 00128 // operational space controllers). 00129 // **************************************************************** 00130 00131 // **************************************************************** 00132 // Feed the output values of the current control cycle back to 00133 // input values of the next control cycle 00134 00135 *IP->CurrentPositionVector = *OP->NewPositionVector ; 00136 *IP->CurrentVelocityVector = *OP->NewVelocityVector ; 00137 *IP->CurrentAccelerationVector = *OP->NewAccelerationVector ; 00138 } 00139 std::cout << "T" << i << " :" << totalt << std::endl; 00140 00141 traj.setWayPointDurationFromPrevious(i, totalt); 00142 traj.getWayPoint(i).getJointStateGroup(traj.getGroup()->getName())->getVariableValues(v); 00143 for (int k = 0 ; k < 7 ; ++k) 00144 IP->TargetPositionVector->VecData[k] = v[k]; 00145 } 00146 } 00147 */ 00148 virtual std::string getDescription() const { return "Add Time Parameterization"; } 00149 00150 virtual bool adaptAndPlan(const PlannerFn &planner, 00151 const planning_scene::PlanningSceneConstPtr& planning_scene, 00152 const planning_interface::MotionPlanRequest &req, 00153 planning_interface::MotionPlanResponse &res, 00154 std::vector<std::size_t> &added_path_index) const 00155 { 00156 bool result = planner(planning_scene, req, res); 00157 if (result && res.trajectory_) 00158 { 00159 ROS_DEBUG("Running '%s'", getDescription().c_str()); 00160 if (!time_param_.computeTimeStamps(*res.trajectory_)) 00161 ROS_WARN("Time parametrization for the solution path failed."); 00162 } 00163 00164 return result; 00165 } 00166 00167 private: 00168 00169 trajectory_processing::IterativeParabolicTimeParameterization time_param_; 00170 }; 00171 00172 } 00173 00174 CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddTimeParameterization, 00175 planning_request_adapter::PlanningRequestAdapter);