Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_
00038 #define MOVEIT_DYNAMICS_SOLVER_DYNAMICS_SOLVER_
00039
00040
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 MOVEIT_CLASS_FORWARD(DynamicsSolver);
00052
00058 class DynamicsSolver
00059 {
00060 public:
00068 DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name,
00069 const geometry_msgs::Vector3& gravity_vector);
00070
00086 bool getTorques(const std::vector<double>& joint_angles, const std::vector<double>& joint_velocities,
00087 const std::vector<double>& joint_accelerations, const std::vector<geometry_msgs::Wrench>& wrenches,
00088 std::vector<double>& torques) const;
00089
00101 bool getMaxPayload(const std::vector<double>& joint_angles, double& payload, unsigned int& joint_saturated) const;
00102
00113 bool getPayloadTorques(const std::vector<double>& joint_angles, double payload,
00114 std::vector<double>& joint_torques) const;
00115
00120 const std::vector<double>& getMaxTorques() const;
00121
00126 const robot_model::RobotModelConstPtr& getRobotModel() const
00127 {
00128 return robot_model_;
00129 }
00130
00131 const robot_model::JointModelGroup* getGroup() const
00132 {
00133 return joint_model_group_;
00134 }
00135
00136 private:
00137 boost::shared_ptr<KDL::ChainIdSolver_RNE> chain_id_solver_;
00138 KDL::Chain kdl_chain_;
00139
00140 robot_model::RobotModelConstPtr robot_model_;
00141 const robot_model::JointModelGroup* joint_model_group_;
00142
00143 robot_state::RobotStatePtr state_;
00144
00145 std::string base_name_, tip_name_;
00146 unsigned int num_joints_, num_segments_;
00147 std::vector<double> max_torques_;
00148
00149 double gravity_;
00150 };
00151 }
00152 #endif