inertia_util.hpp

Go to the documentation of this file.

/*
 * Stanford Whole-Body Control Framework http://stanford-scl.sourceforge.net/
 *
 * Copyright (c) 2010 Stanford University. All rights reserved.
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this program.  If not, see
 * <http://www.gnu.org/licenses/>
 */

#ifndef JSPACE_INERTIA_UTIL_HPP
#define JSPACE_INERTIA_UTIL_HPP

#include <jspace/Model.hpp>
#include <stdexcept>
#include <iosfwd>

class deMatrix3;
class deVector3;
class deQuaternion;
class deFrame;
class deMassProp;

namespace jspace {
  
  void inertia_parallel_axis_transform(double in_ixx, double in_ixy, double in_ixz,
               double in_iyy, double in_iyz, double in_izz,
               double trans_x, double trans_y, double trans_z, double mass,
               double & out_ixx, double & out_ixy, double & out_ixz,
               double & out_iyy, double & out_iyz, double & out_izz);
  
  void inertia_parallel_axis_transform(deMatrix3 const & in_inertia,
               deVector3 const & translation, double mass,
               deMatrix3 & out_inertia);
  
  void inertia_similarity_transform(double in_ixx, double in_ixy, double in_ixz,
            double in_iyy, double in_iyz, double in_izz,
            double rot_qx, double rot_qy, double rot_qz, double rot_qw,
            double & out_ixx, double & out_ixy, double & out_ixz,
            double & out_iyy, double & out_iyz, double & out_izz);
  
  void inertia_similarity_transform(double in_ixx, double in_ixy, double in_ixz,
            double in_iyy, double in_iyz, double in_izz,
            deQuaternion const & rotation,
            deMatrix3 & out_inertia);
  
  
  void inertia_similarity_transform(deMatrix3 const & in_inertia,
            deQuaternion const & rotation,
            deMatrix3 & out_inertia);
  
  void fuse_mass_properties(double orig_mass, deMatrix3 const & orig_inertia, deFrame const & orig_com,
          double adtl_mass, deMatrix3 const & adtl_inertia, deFrame const & adtl_com,
          deFrame const & home_of_adtl_wrt_orig,
          double & fused_mass, deMatrix3 & fused_inertia, deFrame & fused_com);
  
  void fuse_mass_properties(deMassProp /*const*/ & original,
          deMassProp /*const*/ & additional,
          deFrame const & home_of_additional_wrt_original,
          deMassProp & fused);
  
  void mass_inertia_explicit_form(Model const & model, Eigen::MatrixXd & mass_inertia,
          std::ostream * dbgos = 0)
    throw(std::runtime_error);
  
}

#endif // JSPACE_INERTIA_UTIL_HPP