rdl_utils.h

Go to the documentation of this file.

/*
 * Original Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 *
 *
 * RDL - Robot Dynamics Library
 * Modifications Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
 *
 * 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.h"
#include "rdl_dynamics/Quaternion.h"
#include "rdl_dynamics/rdl_mathutils.h"

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, double& mass, 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, double& mass, Math::FramePoint& com, Math::FrameVector* com_velocity = nullptr,
                      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);

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 calcCentroidalMomentumMatrixDot(Model& model, const Math::VectorNd& q, const Math::VectorNd& qdot, Math::MatrixNd& Adot, bool update_kinematics = true);
}  // namespace Utils
}  // namespace RobotDynamics

#endif  // ifndef __RDL_UTILS_H__