kinematic_extension_urdf.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *   http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 
00018 #ifndef COB_TWIST_CONTROLLER_KINEMATIC_EXTENSIONS_KINEMATIC_EXTENSION_URDF_H
00019 #define COB_TWIST_CONTROLLER_KINEMATIC_EXTENSIONS_KINEMATIC_EXTENSION_URDF_H
00020 
00021 #include <string>
00022 #include <vector>
00023 #include <ros/ros.h>
00024 #include <std_msgs/Float64MultiArray.h>
00025 #include <sensor_msgs/JointState.h>
00026 
00027 #include <urdf/model.h>
00028 #include <kdl_parser/kdl_parser.hpp>
00029 #include <Eigen/Geometry>
00030 
00031 #include "cob_twist_controller/kinematic_extensions/kinematic_extension_base.h"
00032 
00033 /* BEGIN KinematicExtensionURDF ****************************************************************************************/
00035 class KinematicExtensionURDF : public KinematicExtensionBase
00036 {
00037     public:
00038         explicit KinematicExtensionURDF(const TwistControllerParams& params)
00039         : KinematicExtensionBase(params)
00040         {}
00041 
00042         ~KinematicExtensionURDF() {}
00043 
00044         bool initExtension();
00045         virtual KDL::Jacobian adjustJacobian(const KDL::Jacobian& jac_chain);
00046         virtual JointStates adjustJointStates(const JointStates& joint_states);
00047         virtual LimiterParams adjustLimiterParams(const LimiterParams& limiter_params);
00048         virtual void processResultExtension(const KDL::JntArray& q_dot_ik);
00049 
00050         void jointstateCallback(const sensor_msgs::JointState::ConstPtr& msg);
00051 
00052     protected:
00053         ros::Publisher command_pub_;
00054         ros::Subscriber joint_state_sub_;
00055 
00056         std::string ext_base_;
00057         std::string ext_tip_;
00058         KDL::Chain chain_;
00059         unsigned int ext_dof_;
00060         std::vector<std::string> joint_names_;
00061         JointStates joint_states_;
00062         std::vector<double> limits_max_;
00063         std::vector<double> limits_min_;
00064         std::vector<double> limits_vel_;
00065         std::vector<double> limits_acc_;
00066 };
00067 /* END KinematicExtensionURDF **********************************************************************************************/
00068 
00069 
00070 /* BEGIN KinematicExtensionTorso ****************************************************************************************/
00072 class KinematicExtensionTorso : public KinematicExtensionURDF
00073 {
00074     public:
00075         explicit KinematicExtensionTorso(const TwistControllerParams& params)
00076         : KinematicExtensionURDF(params)
00077         {
00078             ext_base_ = "torso_base_link";
00079             ext_tip_ = params.chain_base_link;
00080 
00081             if (!initExtension())
00082             {
00083                 ROS_ERROR("Initialization failed");
00084             }
00085 
00086             joint_state_sub_ = nh_.subscribe("/torso/joint_states", 1, &KinematicExtensionURDF::jointstateCallback, dynamic_cast<KinematicExtensionURDF*>(this));
00087             command_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("/torso/joint_group_velocity_controller/command", 1);
00088         }
00089 
00090         ~KinematicExtensionTorso() {}
00091 };
00092 /* END KinematicExtensionTorso **********************************************************************************************/
00093 
00094 #endif  // COB_TWIST_CONTROLLER_KINEMATIC_EXTENSIONS_KINEMATIC_EXTENSION_URDF_H


cob_twist_controller
Author(s): Felix Messmer , Marco Bezzon , Christoph Mark , Francisco Moreno
autogenerated on Thu Jun 6 2019 21:19:26