$search
00001 00013 #include <sstream> 00014 #include <algorithm> 00015 00016 #include <sensor_msgs/JointState.h> 00017 #include <kdl_parser/kdl_parser.hpp> 00018 #include <kdl/jntarray.hpp> 00019 00020 #include <sr_hand/sr_kinematics.h> 00021 #include <sr_hand/sendupdate.h> 00022 #include <sr_hand/joint.h> 00023 00024 using namespace ros; 00025 00026 namespace shadowrobot 00027 { 00028 const std::string SrKinematics::hand_joint_states_topic = "/srh/position/joint_states"; 00029 const std::string SrKinematics::arm_joint_states_topic = "/sr_arm/position/joint_states"; 00030 const std::string SrKinematics::hand_sendupdate_topic = "/srh/sendupdate"; 00031 const std::string SrKinematics::arm_sendupdate_topic = "/sr_arm/sendupdate"; 00032 00033 SrKinematics::SrKinematics() : 00034 n_tilde("~") 00035 { 00039 if( !n_tilde.getParam("ik_service", arm_kinematics_service) ) 00040 { 00041 ROS_FATAL("No service name for reverse kinematics found on parameter server"); 00042 return; 00043 } 00044 rk_client = node.serviceClient<kinematics_msgs::GetPositionIK> (arm_kinematics_service); 00045 00050 pub_hand = node.advertise<sr_hand::sendupdate> (hand_sendupdate_topic, 2); 00051 pub_arm = node.advertise<sr_hand::sendupdate> (arm_sendupdate_topic, 2); 00052 00053 // getting the root and the tip name from the parameter server 00054 if( !n_tilde.getParam("root_name", root_name) ) 00055 { 00056 ROS_FATAL("No root name found on parameter server"); 00057 return; 00058 } 00059 if( !n_tilde.getParam("tip_name", tip_name) ) 00060 { 00061 ROS_FATAL("No tip name found on parameter server"); 00062 return; 00063 } 00064 if( !n_tilde.getParam("rk_target", rk_target) ) 00065 { 00066 ROS_FATAL("No reverse kinematic target found on parameter server"); 00067 return; 00068 } 00069 if( !n_tilde.getParam("fixed_frame", fixed_frame) ) 00070 { 00071 ROS_FATAL("No fixed_frame name found on parameter server"); 00072 return; 00073 } 00074 ROS_INFO("Kinematic chain from %s to %s. Reverse kinematics Target: %s", root_name.c_str(), tip_name.c_str(), rk_target.c_str()); 00075 00076 // get the model from the parameter server 00077 std::string full_param_name, result; 00078 node.searchParam("urdf_description", full_param_name); 00079 n_tilde.param("urdf_description", result, std::string("Failed")); 00080 00081 // Load and Read Models 00082 if( !loadModel(result) ) 00083 { 00084 ROS_FATAL("Could not load models!"); 00085 return; 00086 } 00087 00088 //subscribe to both the arm and hand joint_states to get all the joints angles. 00089 std::string full_topic = hand_joint_states_topic; 00090 hand_subscriber = node.subscribe(full_topic, 10, &SrKinematics::jointstatesCallback, this); 00091 full_topic = arm_joint_states_topic; 00092 arm_subscriber = node.subscribe(full_topic, 10, &SrKinematics::jointstatesCallback, this); 00093 00094 ros::Rate rate = ros::Rate(0.5); 00095 rate.sleep(); 00096 full_topic = "/tf"; 00097 tf_sub = node.subscribe(full_topic, 10, &SrKinematics::reverseKinematicsCallback, this); 00098 } 00099 00100 SrKinematics::~SrKinematics() 00101 { 00102 } 00103 00104 bool SrKinematics::loadModel( const std::string xml ) 00105 { 00106 urdf::Model robot_model; 00107 KDL::Tree tree; 00108 00109 //init the robot_model from the urdf 00110 if( !robot_model.initString(xml) ) 00111 { 00112 ROS_FATAL("Could not initialize robot model"); 00113 return -1; 00114 } 00115 00116 // build the kinematic chain from tip to root. 00117 boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name); 00118 boost::shared_ptr<const urdf::Joint> joint; 00119 while( link && link->name != root_name ) 00120 { 00121 joint = robot_model.getJoint(link->parent_joint->name); 00122 if( !joint ) 00123 { 00124 ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str()); 00125 return false; 00126 } 00127 00128 //add the joint which is in the kinematic chain to the joint map 00129 joints_map[joint->name.c_str()] = 0.0; 00130 ROS_INFO( "adding joint: [%s]", joint->name.c_str() ); 00131 00132 //get the parent link 00133 link = robot_model.getLink(link->getParent()->name); 00134 } 00135 return true; 00136 } 00137 00138 void SrKinematics::jointstatesCallback( const sensor_msgs::JointStateConstPtr& msg ) 00139 { 00140 mutex.lock(); 00141 JointsMap::iterator result; 00142 00143 //update only the joints in the kinematic chain 00144 for( unsigned int index = 0; index < msg->name.size(); ++index ) 00145 { 00146 result = joints_map.find(msg->name[index]); 00147 if( result != joints_map.end() ) 00148 { 00149 joints_map[msg->name[index]] = msg->position[index]; 00150 continue; 00151 } 00152 } 00153 mutex.unlock(); 00154 } 00155 00156 void SrKinematics::reverseKinematicsCallback( const tf::tfMessageConstPtr& msg ) 00157 { 00158 tf::StampedTransform transform; 00159 std::string link_name = msg->transforms[0].child_frame_id; 00160 try 00161 { 00162 //Only compute the reverse kinematics when receiving something from the kinematics target. 00163 if( boost::find_first(link_name, rk_target) ) 00164 { 00165 //the target is publishing the transform between the hand support and the target 00166 // => we need to get the transform from the origin to the kinematics target 00167 // this way the transform and the pose are equal. 00168 tf_listener.lookupTransform(fixed_frame, rk_target, ros::Time(0), transform); 00169 00170 kinematics_msgs::GetPositionIK srv; 00171 srv.request.ik_request.ik_link_name = rk_target; 00172 srv.request.ik_request.pose_stamped = getPoseFromTransform(transform); 00173 srv.request.timeout = ros::Duration(0.1); 00174 00175 mutex.lock(); 00176 //Fill the vector with the current joint positions (only those in the kinematics 00177 // chain). 00178 for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it ) 00179 { 00180 ROS_DEBUG("pos[%s]: %f", it->first.c_str(), it->second); 00181 00182 srv.request.ik_request.ik_seed_state.joint_state.name.push_back(it->first); 00183 srv.request.ik_request.ik_seed_state.joint_state.position.push_back(double(it->second)); 00184 } 00185 mutex.unlock(); 00186 00187 //Call the reverse kinematics service (implemented in arm_kinematics) 00188 if( rk_client.call(srv) ) 00189 { 00190 //The Reverse Kinematics was computed successfully. 00191 // -> send the computed targets to the joints. 00192 sr_hand::sendupdate msg; 00193 std::vector<sr_hand::joint> jointVector; 00194 00195 for( unsigned int i = 0; i < srv.response.solution.joint_state.name.size(); ++i ) 00196 { 00197 std::string sensor_name = srv.response.solution.joint_state.name[i]; 00198 double target = toDegrees(srv.response.solution.joint_state.position[i]); 00199 ROS_DEBUG("[%s] = %f", sensor_name.c_str(), target); 00200 00201 //fill the sendupdate message 00202 sr_hand::joint joint; 00203 joint.joint_name = sensor_name; 00204 joint.joint_target = target; 00205 jointVector.push_back(joint); 00206 } 00207 00208 msg.sendupdate_length = jointVector.size(); 00209 msg.sendupdate_list = jointVector; 00210 00211 //publish the message for both the hand and the arm. 00212 pub_hand.publish(msg); 00213 pub_arm.publish(msg); 00214 } 00215 else 00216 { 00217 ROS_ERROR("cant compute reverse kinematics"); 00218 } 00219 } 00220 } 00221 catch( tf::TransformException ex ) 00222 { 00223 ROS_WARN("%s",ex.what()); 00224 } 00225 } 00226 00227 geometry_msgs::PoseStamped SrKinematics::getPoseFromTransform( tf::StampedTransform transform ) 00228 { 00229 geometry_msgs::PoseStamped pose; 00230 00231 // the transform and the pose are equal, as we're getting the transform from the origin. 00232 pose.header.stamp = ros::Time::now(); 00233 tf::Vector3 position = transform.getOrigin(); 00234 pose.pose.position.x = double(position.x()); 00235 pose.pose.position.y = double(position.y()); 00236 pose.pose.position.z = double(position.z()); 00237 btQuaternion orientation = transform.getRotation(); 00238 pose.pose.orientation.w = double(orientation.w()); 00239 pose.pose.orientation.x = double(orientation.x()); 00240 pose.pose.orientation.y = double(orientation.y()); 00241 pose.pose.orientation.z = double(orientation.z()); 00242 00243 return pose; 00244 } 00245 00246 } 00247 ; //end namespace 00248 00256 int main( int argc, char** argv ) 00257 { 00258 ros::init(argc, argv, "sr_kinematics"); 00259 boost::shared_ptr<shadowrobot::SrKinematics> sr_kin(new shadowrobot::SrKinematics::SrKinematics()); 00260 ros::spin(); 00261 } 00262