Stanford Whole-Body Control Framework: jspace/jspace/inertia

00001 /*
00002  * Stanford Whole-Body Control Framework http://stanford-wbc.sourceforge.net/
00003  *
00004  * Copyright (C) 2010 The Board of Trustees of The Leland Stanford Junior University. All rights reserved.
00005  *
00006  * This program is free software: you can redistribute it and/or
00007  * modify it under the terms of the GNU Lesser General Public License
00008  * as published by the Free Software Foundation, either version 3 of
00009  * the License, or (at your option) any later version.
00010  *
00011  * This program is distributed in the hope that it will be useful, but
00012  * WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  * Lesser General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU Lesser General Public
00017  * License along with this program.  If not, see
00018  * <http://www.gnu.org/licenses/>
00019  */
00020 
00026 #ifndef JSPACE_INERTIA_UTIL_HPP
00027 #define JSPACE_INERTIA_UTIL_HPP
00028 
00029 #include <jspace/Model.hpp>
00030 #include <stdexcept>
00031 #include <iosfwd>
00032 
00033 class deMatrix3;
00034 class deVector3;
00035 class deQuaternion;
00036 class deFrame;
00037 class deMassProp;
00038 
00039 namespace jspace {
00040   
00047   void inertia_parallel_axis_transform(double in_ixx, double in_ixy, double in_ixz,
00048                double in_iyy, double in_iyz, double in_izz,
00049                double trans_x, double trans_y, double trans_z, double mass,
00050                double & out_ixx, double & out_ixy, double & out_ixz,
00051                double & out_iyy, double & out_iyz, double & out_izz);
00052   
00056   void inertia_parallel_axis_transform(deMatrix3 const & in_inertia,
00057                deVector3 const & translation, double mass,
00058                deMatrix3 & out_inertia);
00059   
00066   void inertia_similarity_transform(double in_ixx, double in_ixy, double in_ixz,
00067             double in_iyy, double in_iyz, double in_izz,
00068             double rot_qx, double rot_qy, double rot_qz, double rot_qw,
00069             double & out_ixx, double & out_ixy, double & out_ixz,
00070             double & out_iyy, double & out_iyz, double & out_izz);
00071   
00075   void inertia_similarity_transform(double in_ixx, double in_ixy, double in_ixz,
00076             double in_iyy, double in_iyz, double in_izz,
00077             deQuaternion const & rotation,
00078             deMatrix3 & out_inertia);
00079   
00080   
00084   void inertia_similarity_transform(deMatrix3 const & in_inertia,
00085             deQuaternion const & rotation,
00087             deMatrix3 & out_inertia);
00088   
00099   void fuse_mass_properties(double orig_mass, deMatrix3 const & orig_inertia, deFrame const & orig_com,
00100           double adtl_mass, deMatrix3 const & adtl_inertia, deFrame const & adtl_com,
00101           deFrame const & home_of_adtl_wrt_orig,
00102           double & fused_mass, deMatrix3 & fused_inertia, deFrame & fused_com);
00103   
00107   void fuse_mass_properties(deMassProp /*const*/ & original,
00108           deMassProp /*const*/ & additional,
00109           deFrame const & home_of_additional_wrt_original,
00111           deMassProp & fused);
00112   
00113   void mass_inertia_explicit_form(Model const & model, Matrix & mass_inertia,
00114           std::ostream * dbgos = 0)
00115     throw(std::runtime_error);
00116   
00117 }
00118 
00119 #endif // JSPACE_INERTIA_UTIL_HPP
jspace/jspace/inertia_util.hpp
