Libbarrett  1.2.4
include/barrett/systems/tool_force_to_joint_torques.h
00001 /*
00002         Copyright 2009, 2010 Barrett Technology <support@barrett.com>
00003 
00004         This file is part of libbarrett.
00005 
00006         This version of libbarrett is free software: you can redistribute it
00007         and/or modify it under the terms of the GNU General Public License as
00008         published by the Free Software Foundation, either version 3 of the
00009         License, or (at your option) any later version.
00010 
00011         This version of libbarrett is distributed in the hope that it will be
00012         useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
00013         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014         GNU General Public License for more details.
00015 
00016         You should have received a copy of the GNU General Public License along
00017         with this version of libbarrett.  If not, see
00018         <http://www.gnu.org/licenses/>.
00019 
00020         Further, non-binding information about licensing is available at:
00021         <http://wiki.barrett.com/libbarrett/wiki/LicenseNotes>
00022 */
00023 
00024 /*
00025  * tool_force_to_joint_torques.h
00026  *
00027  *  Created on: Jan 18, 2010
00028  *      Author: dc
00029  */
00030 
00031 #ifndef BARRETT_SYSTEMS_TOOL_FORCE_TO_JOINT_TORQUES_H_
00032 #define BARRETT_SYSTEMS_TOOL_FORCE_TO_JOINT_TORQUES_H_
00033 
00034 
00035 #include <Eigen/Core>
00036 #include <gsl/gsl_blas.h>
00037 
00038 #include <barrett/detail/ca_macro.h>
00039 #include <barrett/units.h>
00040 #include <barrett/math/kinematics.h>
00041 #include <barrett/systems/abstract/single_io.h>
00042 #include <barrett/systems/kinematics_base.h>
00043 
00044 
00045 namespace barrett {
00046 namespace systems {
00047 
00048 
00049 template<size_t DOF>
00050 class ToolForceToJointTorques :
00051         public SingleIO<units::CartesianForce::type, typename units::JointTorques<DOF>::type>,
00052         public KinematicsInput<DOF>
00053 {
00054 
00055         BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
00056 
00057 public:
00058         ToolForceToJointTorques(const std::string& sysName = "ToolForceToJointTorques") :
00059                 SingleIO<cf_type, jt_type>(sysName), KinematicsInput<DOF>(this) {}
00060         virtual ~ToolForceToJointTorques() { this->mandatoryCleanUp(); }
00061 
00062 protected:
00063         jt_type data;
00064 
00065         virtual void operate() {
00066                 // Multiply by the Jacobian-transpose at the tool
00067                 gsl_blas_dgemv(CblasTrans, 1.0,
00068                                 this->kinInput.getValue().impl->tool_jacobian_linear,
00069                                 this->input.getValue().asGslType(), 0.0, data.asGslType());
00070 
00071                 this->outputValue->setData(&data);
00072         }
00073 
00074 private:
00075         DISALLOW_COPY_AND_ASSIGN(ToolForceToJointTorques);
00076 
00077 public:
00078         EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00079 };
00080 
00081 
00082 }
00083 }
00084 
00085 
00086 #endif /* BARRETT_SYSTEMS_TOOL_FORCE_TO_JOINT_TORQUES_H_ */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Defines