kinematic_options.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014, SRI International
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: Acorn Pooley */
00036 
00037 #include <moveit/robot_interaction/kinematic_options.h>
00038 #include <boost/static_assert.hpp>
00039 #include <ros/console.h>
00040 
00041 robot_interaction::KinematicOptions::KinematicOptions()
00042   : timeout_seconds_(0.0)  // 0.0 = use default timeout
00043   , max_attempts_(0)       // 0 = use default max attempts
00044 {
00045 }
00046 
00047 // This is intended to be called as a ModifyStateFunction to modify the state
00048 // maintained by a LockedRobotState in place.
00049 bool robot_interaction::KinematicOptions::setStateFromIK(robot_state::RobotState& state, const std::string& group,
00050                                                          const std::string& tip, const geometry_msgs::Pose& pose) const
00051 {
00052   const robot_model::JointModelGroup* jmg = state.getJointModelGroup(group);
00053   if (!jmg)
00054   {
00055     ROS_ERROR("No getJointModelGroup('%s') found", group.c_str());
00056     return false;
00057   }
00058   bool result = state.setFromIK(jmg, pose, tip, max_attempts_, timeout_seconds_, state_validity_callback_, options_);
00059   state.update();
00060   return result;
00061 }
00062 
00063 void robot_interaction::KinematicOptions::setOptions(const KinematicOptions& source, OptionBitmask fields)
00064 {
00065 // This function is implemented with the O_FIELDS and QO_FIELDS macros to
00066 // ensure that any fields added to robot_interaction::KinematicOptions or
00067 // kinematics::KinematicsQueryOptions are also added here and to the
00068 // KinematicOptions::OptionBitmask enum.
00069 
00070 // This needs to represent all the fields in
00071 // robot_interaction::KinematicOptions except options_
00072 #define O_FIELDS(F)                                                                                                    \
00073   F(double, timeout_seconds_, TIMEOUT)                                                                                 \
00074   F(unsigned int, max_attempts_, MAX_ATTEMPTS)                                                                         \
00075   F(robot_state::GroupStateValidityCallbackFn, state_validity_callback_, STATE_VALIDITY_CALLBACK)
00076 
00077 // This needs to represent all the fields in
00078 // kinematics::KinematicsQueryOptions
00079 #define QO_FIELDS(F)                                                                                                   \
00080   F(bool, lock_redundant_joints, LOCK_REDUNDANT_JOINTS)                                                                \
00081   F(bool, return_approximate_solution, RETURN_APPROXIMATE_SOLUTION)                                                    \
00082   F(::kinematics::DiscretizationMethods::DiscretizationMethod, discretization_method, DISCRETIZATION_METHOD)
00083 
00084   // This structure should be identical to kinematics::KinematicsQueryOptions
00085   // This is only used in the BOOST_STATIC_ASSERT below.
00086   struct DummyKinematicsQueryOptions
00087   {
00088 #define F(type, member, enumval) type member;
00089     QO_FIELDS(F)
00090 #undef F
00091   };
00092   // This structure should be identical to robot_interaction::KinematicOptions
00093   // This is only used in the BOOST_STATIC_ASSERT below.
00094   struct DummyKinematicOptions
00095   {
00096 #define F(type, member, enumval) type member;
00097     O_FIELDS(F)
00098 #undef F
00099     DummyKinematicsQueryOptions options_;
00100   };
00101 
00102   // If these asserts fails it means that fields were added to
00103   // kinematics::KinematicsQueryOptions or robot_interaction::KinematicOptions
00104   // and not added to the O_FIELDS and QO_FIELDS definitions above. To fix add
00105   // any new fields to the definitions above.
00106   BOOST_STATIC_ASSERT(sizeof(kinematics::KinematicsQueryOptions) == sizeof(DummyKinematicsQueryOptions));
00107   BOOST_STATIC_ASSERT(sizeof(KinematicOptions) == sizeof(DummyKinematicOptions));
00108 
00109 // copy fields from other to this if its bit is set in fields
00110 #define F(type, member, enumval)                                                                                       \
00111   if (fields & KinematicOptions::enumval)                                                                              \
00112     member = source.member;
00113   O_FIELDS(F)
00114 #undef F
00115 
00116 // copy fields from other.options_ to this.options_ if its bit is set in
00117 // fields
00118 #define F(type, member, enumval)                                                                                       \
00119   if (fields & KinematicOptions::enumval)                                                                              \
00120     options_.member = source.options_.member;
00121   QO_FIELDS(F)
00122 #undef F
00123 }


robot_interaction
Author(s): Ioan Sucan
autogenerated on Mon Apr 23 2018 10:25:44