00001 #ifndef _PR2_JOINT_SPACE_DDP_EX_ 00002 #define _PR2_JOINT_SPACE_DDP_EX_ 00003 00004 00005 #include <cmath> 00006 #include <algorithm> 00007 #include <string> 00008 00009 #include <ros/ros.h> 00010 00011 #include <kinematics_msgs/GetKinematicSolverInfo.h> 00012 #include <kinematics_msgs/GetPositionFK.h> 00013 #include <kinematics_msgs/GetPositionIK.h> 00014 00015 #include <mpc/types.h> 00016 00017 using namespace MPC; 00018 00019 namespace pr2_joint_space { 00020 00021 class RightArmModel { 00022 private: 00023 // the goal in euclidean coordinates 00024 VectorT goal_; 00025 VectorT res_; 00026 VectorT external_goal_; 00027 ros::NodeHandle &n_; 00028 ros::ServiceClient fk_info_client_; 00029 ros::ServiceClient fk_client_; 00030 ros::ServiceClient ik_client_; 00031 std::vector<std::string> controlled_joints_; 00032 std::vector<int> joint_mapping_; 00033 00034 // cached temporaries 00035 kinematics_msgs::GetKinematicSolverInfo::Request request; 00036 kinematics_msgs::GetKinematicSolverInfo::Response response; 00037 kinematics_msgs::GetPositionFK::Request fk_request; 00038 kinematics_msgs::GetPositionFK::Response fk_response; 00039 kinematics_msgs::GetPositionIK::Request ik_request; 00040 kinematics_msgs::GetPositionIK::Response ik_response; 00041 00042 // some constants 00043 float h; 00044 float c_u; 00045 float c_xT; 00046 float gain; 00047 int num_joints; 00048 std::string root_frame; 00049 00050 public: 00051 RightArmModel(ros::NodeHandle &n, std::string frame); 00052 void setInternalGoal(const VectorT &g); 00053 void setExternalGoal(const VectorT &g); 00054 void dynamics(const VectorT &x, const VectorT &u, VectorT &xNext); 00055 void dynamicsD(const VectorT &x, const VectorT &u, Deriv &d); 00056 float cost(const VectorT &x, const VectorT &u); 00057 00058 void fk(const VectorT &x, VectorT &res); 00059 void ik(const VectorT &pos, VectorT &x); 00060 00061 }; 00062 } 00063 #endif
