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
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_;
00149 KDL::Chain kdl_chain_;
00150
00151 std::string group_name_, base_name_, tip_name_;
00152 unsigned int num_joints_, num_segments_;
00153 std::vector<double> max_torques_;
00154
00155 robot_model::RobotModelConstPtr kinematic_model_;
00156 const robot_model::JointModelGroup* joint_model_group_;
00157
00158 robot_state::RobotStatePtr kinematic_state_;
00159 robot_state::JointStateGroup* joint_state_group_;
00160
00161 double gravity_;
00162
00163 };
00164
00165 typedef boost::shared_ptr<DynamicsSolver> DynamicsSolverPtr;
00166 typedef boost::shared_ptr<const DynamicsSolver> DynamicsSolverConstPtr;
00167
00168 }
00169 #endif