dynamics_solver.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Sachin Chitta */
36 
37 #ifndef MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_
38 #define MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_
39 
40 // KDL
41 #include <kdl/chain.hpp>
42 #include <kdl/chainidsolver_recursive_newton_euler.hpp>
43 
45 #include <geometry_msgs/Vector3.h>
46 #include <geometry_msgs/Wrench.h>
47 
48 #include <memory>
49 
51 namespace dynamics_solver
52 {
54 
61 {
62 public:
70  DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name,
71  const geometry_msgs::Vector3& gravity_vector);
72 
88  bool getTorques(const std::vector<double>& joint_angles, const std::vector<double>& joint_velocities,
89  const std::vector<double>& joint_accelerations, const std::vector<geometry_msgs::Wrench>& wrenches,
90  std::vector<double>& torques) const;
91 
103  bool getMaxPayload(const std::vector<double>& joint_angles, double& payload, unsigned int& joint_saturated) const;
104 
115  bool getPayloadTorques(const std::vector<double>& joint_angles, double payload,
116  std::vector<double>& joint_torques) const;
117 
122  const std::vector<double>& getMaxTorques() const;
123 
128  const robot_model::RobotModelConstPtr& getRobotModel() const
129  {
130  return robot_model_;
131  }
132 
134  {
135  return joint_model_group_;
136  }
137 
138 private:
139  std::shared_ptr<KDL::ChainIdSolver_RNE> chain_id_solver_; // KDL chain inverse dynamics
140  KDL::Chain kdl_chain_; // KDL chain
141 
142  robot_model::RobotModelConstPtr robot_model_;
144 
145  robot_state::RobotStatePtr state_; // robot state
146 
147  std::string base_name_, tip_name_; // base name, tip name
148  unsigned int num_joints_, num_segments_; // number of joints in group, number of segments in group
149  std::vector<double> max_torques_; // vector of max torques
150 
151  double gravity_; // Norm of the gravity vector passed in initialize()
152 };
153 }
154 #endif
const robot_model::JointModelGroup * getGroup() const
Core components of MoveIt!
bool getPayloadTorques(const std::vector< double > &joint_angles, double payload, std::vector< double > &joint_torques) const
Get torques corresponding to a particular payload value. Payload is the weight that this group can ho...
std::shared_ptr< KDL::ChainIdSolver_RNE > chain_id_solver_
robot_model::RobotModelConstPtr robot_model_
const std::vector< double > & getMaxTorques() const
Get maximum torques for this group.
This namespace includes the dynamics_solver library.
MOVEIT_CLASS_FORWARD(DynamicsSolver)
robot_state::RobotStatePtr state_
const robot_model::RobotModelConstPtr & getRobotModel() const
Get the kinematic model.
const robot_model::JointModelGroup * joint_model_group_
bool getTorques(const std::vector< double > &joint_angles, const std::vector< double > &joint_velocities, const std::vector< double > &joint_accelerations, const std::vector< geometry_msgs::Wrench > &wrenches, std::vector< double > &torques) const
Get the torques (the order of all input and output is the same as the order of joints for this group ...
std::vector< double > max_torques_
bool getMaxPayload(const std::vector< double > &joint_angles, double &payload, unsigned int &joint_saturated) const
Get the maximum payload for this group (in kg). Payload is the weight that this group can hold when t...
DynamicsSolver(const robot_model::RobotModelConstPtr &robot_model, const std::string &group_name, const geometry_msgs::Vector3 &gravity_vector)
Initialize the dynamics solver.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05