Libbarrett: include/barrett/math/detail/kinematics-inl.h Source File

00001 /*
00002  * kinematics-inl.h
00003  *
00004  *  Created on: Jan 14, 2010
00005  *      Author: dc
00006  */
00007 
00008 #include <libconfig.h++>
00009 
00010 #include <barrett/units.h>
00011 #include <barrett/cdlbt/kinematics.h>
00012 
00013 
00014 namespace barrett {
00015 namespace math {
00016 
00017 
00018 template<size_t DOF>
00019 Kinematics<DOF>::Kinematics(const libconfig::Setting& setting)
00020 {
00021         if (bt_kinematics_create(&impl, setting.getCSetting(), DOF)) {
00022                 throw(std::runtime_error("(math::Kinematics::Kinematics): Couldn't initialize Kinematics struct."));
00023         }
00024 }
00025 
00026 template<size_t DOF>
00027 Kinematics<DOF>::~Kinematics()
00028 {
00029         bt_kinematics_destroy(impl);
00030 }
00031 
00032 template<size_t DOF>
00033 void Kinematics<DOF>::eval(const jp_type& jp, const jv_type& jv)
00034 {
00035         bt_kinematics_eval(impl, jp.asGslType(), jv.asGslType());
00036 }
00037 
00038 template<size_t DOF>
00039 units::CartesianPosition::type Kinematics<DOF>::operator() (const boost::tuple<jp_type, jv_type>& jointState)
00040 {
00041         eval(boost::tuples::get<0>(jointState), boost::tuples::get<1>(jointState));
00042         return units::CartesianPosition::type(impl->tool->origin_pos);
00043 }
00044 
00045 
00046 }
00047 }