rdl_utils.hpp

Go to the documentation of this file.

// Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
// Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
// RDL - Robot Dynamics Library
// Licensed under the zlib license. See LICENSE for more details.
 
#ifndef __RDL_UTILS_H__
#define __RDL_UTILS_H__
 
#include <string>
 
#include "rdl_dynamics/FramePoint.hpp"
#include "rdl_dynamics/Model.hpp"
#include "rdl_dynamics/rdl_mathutils.hpp"
 
namespace RobotDynamics
{
struct Model;
 
namespace Utils
{
std::string getDofName(const Math::SpatialVector& joint_dof);
 
std::string getBodyName(const RobotDynamics::Model& model, unsigned int body_id);
 
std::string getModelHierarchy(const Model& model);
 
std::string getModelDOFOverview(const Model& model);
 
std::string getNamedBodyOriginsOverview(Model& model);
 
void calcCenterOfMass(Model& model, const Math::VectorNd& q, Math::Vector3d& com, bool update_kinematics = true);
 
void calcCenterOfMass(Model& model, const Math::VectorNd& q, Math::FramePoint& com, bool update_kinematics = true);
 
void calcCenterOfMass(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::Vector3d& com, Math::Vector3d* com_velocity = NULL,
                      Math::Vector3d* angular_momentum = NULL, bool update_kinematics = true);
 
void calcCenterOfMass(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::FramePoint& com, Math::FrameVector& com_velocity,
                      Math::FrameVector* angular_momentum = nullptr, bool update_kinematics = true);
 
void calcCenterOfMass(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::FramePoint& com, Math::FrameVector* com_velocity = nullptr,
                      bool update_kinematics = true);
 
void calcCenterOfMass(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::Vector3d& com, Math::Vector3d& com_velocity,
                      bool update_kinematics = true);
 
RobotDynamics::Math::FramePoint updateCenterOfMassFrame(Model& model, const Math::VectorNd& q, bool update_kinematics = true);
 
RobotDynamics::Math::FramePoint updateCenterOfMassFrame(Model& model, const Math::VectorNd& q, const Math::Vector3d& euler_ypr, bool update_kinematics = true);
 
RobotDynamics::Math::FramePoint updateCenterOfMassFrame(Model& model, const Math::VectorNd& q, const Math::Quaternion& orientation, bool update_kinematics = true);
 
void updateCenterOfMassFrame(Model& model, const Math::Vector3d& p_com);
 
void updateCenterOfMassFrame(Model& model, const Math::Vector3d& p_com, const Math::Vector3d& euler_ypr);
 
void updateCenterOfMassFrame(Model& model, const Math::Vector3d& p_com, const Math::Quaternion& orientation);
 
void calcCenterOfMassVelocity(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::Vector3d& com_vel, bool update_kinematics = true);
 
void calcCenterOfMassVelocity(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::FrameVector& com_vel, bool update_kinematics = true);
 
void calcCenterOfMassVelocity(Model& model, Math::Vector3d& com_vel);
 
void calcCenterOfMassVelocity(Model& model, Math::FrameVector& com_vel);
 
double calcPotentialEnergy(Model& model, const Math::VectorNd& q, bool update_kinematics = true);
 
double calcKineticEnergy(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, bool update_kinematics = true);
 
Math::SpatialForce calcGravityWrenchOnCenterOfMass(Model& model, const Math::VectorNd& q, bool update_kinematics = true);
 
void calcCenterOfMassJacobian(Model& model, const Math::VectorNd& q, Math::MatrixNd& jCom, bool update_kinematics = true);
 
Math::Vector3d calcSubtreeCenterOfMassScaledByMass(Model& model, const unsigned int bodyId, const Math::VectorNd& q, bool updateKinematics = true);
 
double calcSubtreeMass(Model& model, const unsigned int bodyId);
 
void calcCentroidalMomentumMatrix(Model& model, const Math::VectorNd& q, Math::MatrixNd& A, bool update_kinematics = true);
 
void calcCentroidalMomentumMatrix(Model& model, const Math::VectorNd& q, Math::MatrixNd& A, Math::Vector3d com, bool update_kinematics = true);
 
void calcCentroidalMomentumMatrixDot(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::MatrixNd& Adot, Math::Vector3d com,
                                     Math::Vector3d com_velocity, bool update_kinematics = true);
 
void calcCentroidalMomentumMatrixDot(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::MatrixNd& Adot, bool update_kinematics = true);
 
void calcCentroidalMomentumMatrixAndMatrixDot(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::MatrixNd& A, Math::MatrixNd& Adot,
                                              Math::Vector3d com, Math::Vector3d com_velocity, bool update_kinematics = true);
 
void calcCentroidalMomentumMatrixAndMatrixDot(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::MatrixNd& A, Math::MatrixNd& Adot,
                                              bool update_kinematics = true);
 
}  // namespace Utils
}  // namespace RobotDynamics
 
#endif  // ifndef __RDL_UTILS_H__