debug_utils.h

Go to the documentation of this file.

/*******************************************************************/
/*    AUTHOR: Paal Arthur S. Thorseth                              */
/*    ORGN:   Dept of Eng Cybernetics, NTNU Trondheim              */
/*    FILE:   debug_utils.h                                        */
/*    DATE:   Apr 12, 2021                                         */
/*                                                                 */
/* Copyright (C) 2021 Paal Arthur S. Thorseth,                     */
/*                    Adrian B. Ghansah                            */
/*                                                                 */
/* This program is free software: you can redistribute it          */
/* and/or modify it under the terms of the GNU 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 General Public License for more details.            */
/*                                                                 */
/* You should have received a copy of the GNU General Public       */
/* License along with this program. If not, see                    */
/* <https://www.gnu.org/licenses/>.                                */
/*                                                                 */
/*******************************************************************/
 
#pragma once
 
// C++ Standard Libraries
#include <cmath>
 
// ROS
#include "ros/ros.h"
 
// ROS Package Libraries
#include <math_utils/angle_utils.h>
 
// Eigen
#include <Eigen/Core>
 
namespace debug_utils
{
    void printBaseState(const Eigen::Matrix<double, 6, 1> &_base_state);
 
    void printBaseTwist(const Eigen::Matrix<double, 6, 1> &_base_twist);
 
    void printBaseAccel(const Eigen::Matrix<double, 6, 1> &_base_accel);
 
    void printJointState(const Eigen::Matrix<double, 12, 1> &_joint_state);
 
    void printJointVelocities(const Eigen::Matrix<double, 12, 1> &_joint_vel);
 
    void printJointAccelerations(const Eigen::Matrix<double, 12, 1> &_joint_vel);
 
    void printJointTorques(const Eigen::Matrix<double, 12, 1> &_joint_trq);
 
    void printGeneralizedCoordinates(const Eigen::Matrix<double, 18, 1> &_gen_coord);
 
    void printGeneralizedVelocities(const Eigen::Matrix<double, 18, 1> &_gen_vel);
 
    void printGeneralizedAccelerations(const Eigen::Matrix<double, 18, 1> &_gen_accel);
 
    void printFootstepPositions(const Eigen::Matrix<Eigen::Vector3d, 4, 1> &_f_pos);
 
    void printFootstepForces(const int* _contact_state,
                             const Eigen::VectorXd &_F_c);
 
 
} // namespace debug_utils