sr_kinematics.cpp
Go to the documentation of this file.
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 


sr_hand
Author(s): Ugo Cupcic / ugo@shadowrobot.com, contact@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:44:02