dynamics_solver.h
Go to the documentation of this file.
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 Inc. 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 * Author: Sachin Chitta
00035 *********************************************************************/
00036 
00037 #ifndef MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_
00038 #define MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_
00039 
00040 // KDL
00041 #include <kdl/chain.hpp>
00042 #include <kdl/chainidsolver_recursive_newton_euler.hpp>
00043 
00044 #include <moveit/robot_state/robot_state.h>
00045 #include <geometry_msgs/Vector3.h>
00046 #include <geometry_msgs/Wrench.h>
00047 
00049 namespace dynamics_solver
00050 {
00051 
00057 class DynamicsSolver
00058 {
00059 public:
00060 
00068   DynamicsSolver(const robot_model::RobotModelConstPtr &kinematic_model,
00069                  const std::string &group_name,
00070                  const geometry_msgs::Vector3 &gravity_vector);
00071 
00087   bool getTorques(const std::vector<double> &joint_angles,
00088                   const std::vector<double> &joint_velocities,
00089                   const std::vector<double> &joint_accelerations,
00090                   const std::vector<geometry_msgs::Wrench> &wrenches,
00091                   std::vector<double> &torques) const;
00092 
00104   bool getMaxPayload(const std::vector<double> &joint_angles,
00105                      double &payload,
00106                      unsigned int &joint_saturated) const;
00107 
00118   bool getPayloadTorques(const std::vector<double> &joint_angles,
00119                          double payload,
00120                          std::vector<double> &joint_torques) const;
00121 
00126   const std::vector<double>& getMaxTorques() const;
00127 
00132   const robot_model::RobotModelConstPtr& getRobotModel() const
00133   {
00134     return kinematic_model_;
00135   }
00136 
00141   const std::string& getGroupName() const
00142   {
00143     return group_name_;
00144   }
00145 
00146 private:
00147 
00148   boost::shared_ptr<KDL::ChainIdSolver_RNE> chain_id_solver_; // KDL chain inverse dynamics
00149   KDL::Chain kdl_chain_; // KDL chain
00150 
00151   std::string group_name_, base_name_, tip_name_; // group name, base name, tip name
00152   unsigned int num_joints_, num_segments_; // number of joints in group, number of segments in group
00153   std::vector<double> max_torques_; // vector of max torques
00154 
00155   robot_model::RobotModelConstPtr kinematic_model_; // kinematic model
00156   const robot_model::JointModelGroup* joint_model_group_; //joint model group
00157 
00158   robot_state::RobotStatePtr kinematic_state_; //kinematic state
00159   robot_state::JointStateGroup* joint_state_group_; //joint state for the group
00160 
00161   double gravity_; //Norm of the gravity vector passed in initialize()
00162 
00163 };
00164 
00165 typedef boost::shared_ptr<DynamicsSolver> DynamicsSolverPtr;
00166 typedef boost::shared_ptr<const DynamicsSolver> DynamicsSolverConstPtr;
00167 
00168 }
00169 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Oct 6 2014 02:24:46