pr2_kdl_wrapper_example.cpp
Go to the documentation of this file.
00001 /*
00002  * pr2_kdl_wrapper_example.cpp
00003  *
00004  *  Created on: Nov 8, 2013
00005  *      Author: Francisco Vina
00006  */
00007 
00008 /* Copyright (c) 2013, Francisco Vina, CVAP, KTH
00009    All rights reserved.
00010 
00011    Redistribution and use in source and binary forms, with or without
00012    modification, are permitted provided that the following conditions are met:
00013       * Redistributions of source code must retain the above copyright
00014         notice, this list of conditions and the following disclaimer.
00015       * Redistributions in binary form must reproduce the above copyright
00016         notice, this list of conditions and the following disclaimer in the
00017         documentation and/or other materials provided with the distribution.
00018       * Neither the name of KTH nor the
00019         names of its contributors may be used to endorse or promote products
00020         derived from this software without specific prior written permission.
00021 
00022    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00023    ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00024    WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00025    DISCLAIMED. IN NO EVENT SHALL KTH BE LIABLE FOR ANY
00026    DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00027    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028    LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00029    ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00030    (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00031    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00032 */
00033 
00034 #include <ros/ros.h>
00035 #include <ros/console.h>
00036 #include <iostream>
00037 #include <kdl_wrapper/kdl_wrapper.h>
00038 
00039 
00040 int main(int argc, char *argv[])
00041 {
00043     ros::init(argc, argv, "pr2_kdl_wrapper_example");
00044 
00045 
00046     KDLWrapper pr2_kdl_wrapper;
00047 
00048     if(!pr2_kdl_wrapper.init("torso_lift_link", "r_gripper_tool_frame"))
00049     {
00050         ROS_ERROR("Error initiliazing pr2_kdl_wrapper");
00051     }
00052 
00053     KDL::JntArray q_in(7);
00054     q_in(0) = 0.0;
00055     q_in(1) = M_PI/2;
00056     q_in(2) = 0.0;
00057     q_in(3) = M_PI/4;
00058     q_in(4) = 0.0;
00059     q_in(5) = 0.0;
00060     q_in(6) = M_PI/3;
00061 
00062 
00063     KDL::Twist v_in;
00064     v_in.vel = KDL::Vector(0.0, 0.2, 0.2);
00065     v_in.rot = KDL::Vector(0.0, 0.0, 0.0);
00066 
00067     KDL::JntArray q_dot_out;
00068     ROS_INFO("Calculating inverse kinematics");
00069     pr2_kdl_wrapper.ik_solver_vel->setLambda(0.3);
00070     pr2_kdl_wrapper.ik_solver_vel->CartToJnt(q_in, v_in, q_dot_out);
00071 
00072     ROS_INFO("Output q_dot: (%f, %f, %f, %f, %f, %f, %f)",
00073              q_dot_out(0),
00074              q_dot_out(1),
00075              q_dot_out(2),
00076              q_dot_out(3),
00077              q_dot_out(4),
00078              q_dot_out(5),
00079              q_dot_out(6));
00080 
00081     return 0;
00082 }


kdl_wrapper
Author(s): Francisco Vina
autogenerated on Wed Aug 26 2015 12:14:30