462 lines
16 KiB
C++
462 lines
16 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
||
|
|
*
|
||
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||
|
|
* you may not use this file except in compliance with the License.
|
||
|
|
* You may obtain a copy of the License at
|
||
|
|
*
|
||
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||
|
|
*
|
||
|
|
* Unless required by applicable law or agreed to in writing, software
|
||
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||
|
|
* See the License for the specific language governing permissions and
|
||
|
|
* limitations under the License.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
#ifndef _GAZEBO_SKELETON_HH_
|
||
|
|
#define _GAZEBO_SKELETON_HH_
|
||
|
|
|
||
|
|
#include <vector>
|
||
|
|
#include <string>
|
||
|
|
#include <map>
|
||
|
|
#include <utility>
|
||
|
|
|
||
|
|
#include <ignition/math/Matrix4.hh>
|
||
|
|
|
||
|
|
#include "gazebo/math/Matrix4.hh"
|
||
|
|
#include "gazebo/common/CommonTypes.hh"
|
||
|
|
#include "gazebo/util/system.hh"
|
||
|
|
|
||
|
|
namespace gazebo
|
||
|
|
{
|
||
|
|
namespace common
|
||
|
|
{
|
||
|
|
class SkeletonNode;
|
||
|
|
class NodeTransform;
|
||
|
|
class SkeletonAnimation;
|
||
|
|
|
||
|
|
typedef std::map<unsigned int, SkeletonNode*> NodeMap;
|
||
|
|
typedef std::map<unsigned int, SkeletonNode*>::iterator NodeMapIter;
|
||
|
|
|
||
|
|
typedef std::map<double, std::vector<NodeTransform> > RawNodeAnim;
|
||
|
|
typedef std::map<std::string, RawNodeAnim> RawSkeletonAnim;
|
||
|
|
|
||
|
|
typedef std::vector<std::vector<std::pair<std::string, double> > >
|
||
|
|
RawNodeWeights;
|
||
|
|
|
||
|
|
/// \addtogroup gazebo_common Common Animation
|
||
|
|
/// \{
|
||
|
|
|
||
|
|
/// \class Skeleton Skeleton.hh common/common.hh
|
||
|
|
/// \brief A skeleton
|
||
|
|
class GZ_COMMON_VISIBLE Skeleton
|
||
|
|
{
|
||
|
|
/// \brief Constructor
|
||
|
|
public: Skeleton();
|
||
|
|
|
||
|
|
/// \brief Constructor
|
||
|
|
/// \param[in] _root node
|
||
|
|
public: Skeleton(SkeletonNode *_root);
|
||
|
|
|
||
|
|
/// \brief Destructor
|
||
|
|
public: virtual ~Skeleton();
|
||
|
|
|
||
|
|
/// \brief Change the root node
|
||
|
|
/// \param[in] _node the new node
|
||
|
|
public: void SetRootNode(SkeletonNode* _node);
|
||
|
|
|
||
|
|
/// \brief Return the root
|
||
|
|
/// \return the root
|
||
|
|
public: SkeletonNode* GetRootNode();
|
||
|
|
|
||
|
|
/// \brief Find a node
|
||
|
|
/// \param[in] _name the name of the node to look for
|
||
|
|
/// \return the node, or NULL if not found
|
||
|
|
public: SkeletonNode* GetNodeByName(std::string _name);
|
||
|
|
|
||
|
|
/// \brief Find node by index
|
||
|
|
/// \param[in] _id the index
|
||
|
|
/// \return the node, or NULL if not found
|
||
|
|
public: SkeletonNode* GetNodeById(std::string _id);
|
||
|
|
|
||
|
|
/// \brief Find or create node with handle
|
||
|
|
/// \param[in] _handle
|
||
|
|
/// \return the node. A new node is created if it didn't exist
|
||
|
|
public: SkeletonNode* GetNodeByHandle(unsigned int _handle);
|
||
|
|
|
||
|
|
/// \brief Returns the node count
|
||
|
|
/// \return the count
|
||
|
|
public: unsigned int GetNumNodes();
|
||
|
|
|
||
|
|
/// \brief Returns the number of joints
|
||
|
|
/// \return the count
|
||
|
|
public: unsigned int GetNumJoints();
|
||
|
|
|
||
|
|
/// \brief Scale all nodes, transforms and animation data
|
||
|
|
/// \param[in] the scaling factor
|
||
|
|
public: void Scale(double _scale);
|
||
|
|
|
||
|
|
/// \brief Set the bind pose skeletal transform
|
||
|
|
/// \param[in] _trans the transform
|
||
|
|
public: void SetBindShapeTransform(
|
||
|
|
const ignition::math::Matrix4d &_trans);
|
||
|
|
|
||
|
|
/// \brief Return bind pose skeletal transform
|
||
|
|
/// \return a matrix
|
||
|
|
public: ignition::math::Matrix4d BindShapeTransform();
|
||
|
|
|
||
|
|
/// \brief Outputs the transforms to std::err stream
|
||
|
|
public: void PrintTransforms();
|
||
|
|
|
||
|
|
/// \brief Get a copy or the node dictionary
|
||
|
|
public: NodeMap GetNodes();
|
||
|
|
|
||
|
|
/// \brief Resizes the raw node weight array
|
||
|
|
/// \param[in] _vertices the new size
|
||
|
|
public: void SetNumVertAttached(unsigned int _vertices);
|
||
|
|
|
||
|
|
/// \brief Add a new weight to a node (bone)
|
||
|
|
/// \param[in] _vertex index of the vertex
|
||
|
|
/// \param[in] _node name of the bone
|
||
|
|
/// \param[in] _weight the new weight (range 0 to 1)
|
||
|
|
public: void AddVertNodeWeight(unsigned int _vertex, std::string _node,
|
||
|
|
double _weight);
|
||
|
|
|
||
|
|
/// \brief Returns the number of bone weights for a vertex
|
||
|
|
/// \param[in] _vertex the index of the vertex
|
||
|
|
/// \return the count
|
||
|
|
public: unsigned int GetNumVertNodeWeights(unsigned int _vertex);
|
||
|
|
|
||
|
|
/// \brief Weight of a bone for a vertex
|
||
|
|
/// \param[in] _v the index of the vertex
|
||
|
|
/// \param[in] _i the index of the weight for that vertex
|
||
|
|
/// \return a pair containing the name of the node and the weight
|
||
|
|
public: std::pair<std::string, double> GetVertNodeWeight(unsigned int _v,
|
||
|
|
unsigned int _i);
|
||
|
|
|
||
|
|
/// \brief Returns the number of animations
|
||
|
|
/// \return the count
|
||
|
|
public: unsigned int GetNumAnimations();
|
||
|
|
|
||
|
|
/// \brief Find animation
|
||
|
|
/// \param[in] _i the animation index
|
||
|
|
/// \return the animation, or NULL if _i is out of bounds
|
||
|
|
public: SkeletonAnimation* GetAnimation(const unsigned int _i);
|
||
|
|
|
||
|
|
/// \brief Add an animation. The skeleton does not take ownership of the
|
||
|
|
/// animation
|
||
|
|
/// \param[in] _anim the animation to add
|
||
|
|
public: void AddAnimation(SkeletonAnimation *_anim);
|
||
|
|
|
||
|
|
/// \brief Initializes the hande numbers for each node in the map
|
||
|
|
/// using breadth first traversal
|
||
|
|
protected: void BuildNodeMap();
|
||
|
|
|
||
|
|
/// \brief the root node
|
||
|
|
protected: SkeletonNode *root;
|
||
|
|
|
||
|
|
/// \brief The dictionary of nodes, indexed by name
|
||
|
|
protected: NodeMap nodes;
|
||
|
|
|
||
|
|
/// \brief the bind pose skeletal transform
|
||
|
|
protected: ignition::math::Matrix4d bindShapeTransform;
|
||
|
|
|
||
|
|
/// \brief the node weight table
|
||
|
|
protected: RawNodeWeights rawNW;
|
||
|
|
|
||
|
|
/// \brief the array of animations
|
||
|
|
protected: std::vector<SkeletonAnimation*> anims;
|
||
|
|
};
|
||
|
|
|
||
|
|
/// \class SkeletonNode Skeleton.hh common/common.hh
|
||
|
|
/// \brief A skeleton node
|
||
|
|
class GZ_COMMON_VISIBLE SkeletonNode
|
||
|
|
{
|
||
|
|
/// \brief enumeration of node types
|
||
|
|
public: enum SkeletonNodeType {NODE, JOINT};
|
||
|
|
|
||
|
|
/// \brief Constructor
|
||
|
|
/// \param[in] _parent The parent node
|
||
|
|
public: SkeletonNode(SkeletonNode* _parent);
|
||
|
|
|
||
|
|
/// \brief Constructor
|
||
|
|
/// \param[in] _parent the parent node
|
||
|
|
/// \param[in] _name name of node
|
||
|
|
/// \param[in] _id Id of node
|
||
|
|
/// \param[in] _type The type of this node
|
||
|
|
public: SkeletonNode(SkeletonNode* _parent, std::string _name,
|
||
|
|
std::string _id, SkeletonNodeType _type = JOINT);
|
||
|
|
|
||
|
|
/// \brief Destructor
|
||
|
|
public: virtual ~SkeletonNode();
|
||
|
|
|
||
|
|
/// \brief Change the name
|
||
|
|
/// \param[in] _name the new name
|
||
|
|
public: void SetName(std::string _name);
|
||
|
|
|
||
|
|
/// \brief Returns the name
|
||
|
|
/// \return the name
|
||
|
|
public: std::string GetName();
|
||
|
|
|
||
|
|
/// \brief Change the id string
|
||
|
|
/// \param[in] _id the new id string
|
||
|
|
public: void SetId(std::string _id);
|
||
|
|
|
||
|
|
/// \brief Returns the index
|
||
|
|
/// \return the id string
|
||
|
|
public: std::string GetId();
|
||
|
|
|
||
|
|
/// \brief Change the skeleton node type
|
||
|
|
/// \param[in] _type the new type
|
||
|
|
public: void SetType(SkeletonNodeType _type);
|
||
|
|
|
||
|
|
/// \brief Is a joint query
|
||
|
|
/// \return true if the skeleton type is a joint, false otherwise
|
||
|
|
public: bool IsJoint();
|
||
|
|
|
||
|
|
/// \brief Set a transformation
|
||
|
|
/// \param[in] _trans the transformation
|
||
|
|
/// \param[in] _updateChildren when true the UpdateChildrenTransforms
|
||
|
|
/// operation is performed
|
||
|
|
public: void SetTransform(const ignition::math::Matrix4d &_trans,
|
||
|
|
bool _updateChildren = true);
|
||
|
|
|
||
|
|
/// \brief Set the model transformation
|
||
|
|
/// \param[in] _trans the transformation
|
||
|
|
/// \param[in] _updateChildren when true the UpdateChildrenTransforms
|
||
|
|
/// operation is performed
|
||
|
|
public: void SetModelTransform(const ignition::math::Matrix4d &_trans,
|
||
|
|
bool _updateChildren = true);
|
||
|
|
|
||
|
|
/// \brief Apply model transformations in order for each node in the tree
|
||
|
|
public: void UpdateChildrenTransforms();
|
||
|
|
|
||
|
|
/// \brief Sets the initial transformation
|
||
|
|
/// \param[in] _tras the transfromation matrix
|
||
|
|
public: void SetInitialTransform(const ignition::math::Matrix4d &_tras);
|
||
|
|
|
||
|
|
/// \brief Reset the transformation to the initial transformation
|
||
|
|
/// \param[in] _resetChildren when true, performs the operation for every
|
||
|
|
/// node in the tree
|
||
|
|
public: void Reset(bool _resetChildren);
|
||
|
|
|
||
|
|
/// \brief Get transform relative to parent
|
||
|
|
/// \return Transform relative to parent
|
||
|
|
public: ignition::math::Matrix4d Transform();
|
||
|
|
|
||
|
|
/// \brief Set the parent node
|
||
|
|
/// \param[in] _parent the new parent
|
||
|
|
public: void SetParent(SkeletonNode* _parent);
|
||
|
|
|
||
|
|
/// \brief Returns the parent node
|
||
|
|
/// \return the parent
|
||
|
|
public: SkeletonNode* GetParent();
|
||
|
|
|
||
|
|
/// \brief Queries wether a node has no parent parent
|
||
|
|
/// \return true if the node has no parent, fasle otherwise
|
||
|
|
public: bool IsRootNode();
|
||
|
|
|
||
|
|
/// \brief Add a new child
|
||
|
|
/// \param[in] _child a child
|
||
|
|
public: void AddChild(SkeletonNode* _child);
|
||
|
|
|
||
|
|
/// \brief Returns the children count
|
||
|
|
/// \return the count
|
||
|
|
public: unsigned int GetChildCount();
|
||
|
|
|
||
|
|
/// \brief Find a child by index
|
||
|
|
/// \param[in] _index the index
|
||
|
|
/// \return the child skeleton. NO BOUNDS CHECKING
|
||
|
|
public: SkeletonNode* GetChild(unsigned int _index);
|
||
|
|
|
||
|
|
/// \brief Get child by name
|
||
|
|
/// \param[in] _name the name of the child skeleton
|
||
|
|
/// \return the skeleton, or NULL if not found
|
||
|
|
public: SkeletonNode* GetChildByName(std::string _name);
|
||
|
|
|
||
|
|
/// \brief Get child by string id
|
||
|
|
/// \param[in] _id the string id
|
||
|
|
/// \return the child skeleton or NULL if not found
|
||
|
|
public: SkeletonNode* GetChildById(std::string _id);
|
||
|
|
|
||
|
|
/// \brief Assign a handle number
|
||
|
|
/// \param[in] _h the handle
|
||
|
|
public: void SetHandle(unsigned int _h);
|
||
|
|
|
||
|
|
/// \brief Get the handle index
|
||
|
|
/// \return the handle index
|
||
|
|
public: unsigned int GetHandle();
|
||
|
|
|
||
|
|
/// \brief Assign the inverse of the bind pose skeletal transform
|
||
|
|
/// \param[in] _invBM the transform
|
||
|
|
public: void SetInverseBindTransform(
|
||
|
|
const ignition::math::Matrix4d &_invBM);
|
||
|
|
|
||
|
|
/// \brief Retrieve the inverse of the bind pose skeletal transform
|
||
|
|
/// \return the transform
|
||
|
|
public: ignition::math::Matrix4d InverseBindTransform();
|
||
|
|
|
||
|
|
/// \brief Retrieve the model transform
|
||
|
|
/// \return the transform
|
||
|
|
public: ignition::math::Matrix4d ModelTransform() const;
|
||
|
|
|
||
|
|
/// \brief Retrieve the raw transformations
|
||
|
|
/// \return an array of transformations
|
||
|
|
public: std::vector<NodeTransform> GetRawTransforms();
|
||
|
|
|
||
|
|
/// \brief Return the raw transformations count
|
||
|
|
/// \return the count
|
||
|
|
public: unsigned int GetNumRawTrans();
|
||
|
|
|
||
|
|
/// \brief Find a raw transformation
|
||
|
|
/// \param[in] _i the index of the transformation
|
||
|
|
/// \return the node transform. NO BOUNDS CHECKING PERFORMED
|
||
|
|
public: NodeTransform GetRawTransform(unsigned int _i);
|
||
|
|
|
||
|
|
/// \brief Add a raw transform
|
||
|
|
/// \param[in] _t the transform
|
||
|
|
public: void AddRawTransform(NodeTransform _t);
|
||
|
|
|
||
|
|
/// \brief Returns a copy of the array of transformations.
|
||
|
|
/// \return the array of transform (These are the same as the raw trans)
|
||
|
|
public: std::vector<NodeTransform> GetTransforms();
|
||
|
|
|
||
|
|
/// \brief the name of the skeletal node
|
||
|
|
protected: std::string name;
|
||
|
|
|
||
|
|
/// \brief a string identifier
|
||
|
|
protected: std::string id;
|
||
|
|
|
||
|
|
/// \brief the type fo node
|
||
|
|
protected: SkeletonNodeType type;
|
||
|
|
|
||
|
|
/// \brief the transform
|
||
|
|
protected: ignition::math::Matrix4d transform;
|
||
|
|
|
||
|
|
/// \brief the initial transformation
|
||
|
|
protected: ignition::math::Matrix4d initialTransform;
|
||
|
|
|
||
|
|
/// \brief the model transformation
|
||
|
|
protected: ignition::math::Matrix4d modelTransform;
|
||
|
|
|
||
|
|
/// \brief the inverse of the bind pose skeletal transform
|
||
|
|
protected: ignition::math::Matrix4d invBindTransform;
|
||
|
|
|
||
|
|
/// \brief the parent node
|
||
|
|
protected: SkeletonNode *parent;
|
||
|
|
|
||
|
|
/// \brief the children nodes
|
||
|
|
protected: std::vector<SkeletonNode*> children;
|
||
|
|
|
||
|
|
/// \brief handle index number
|
||
|
|
protected: unsigned int handle;
|
||
|
|
|
||
|
|
/// \brief the raw transformation
|
||
|
|
protected: std::vector<NodeTransform> rawTransforms;
|
||
|
|
};
|
||
|
|
|
||
|
|
/// \clas NodeTransform Skeleton.hh common/common.hh
|
||
|
|
/// \brief A transformation node
|
||
|
|
class GZ_COMMON_VISIBLE NodeTransform
|
||
|
|
{
|
||
|
|
/// \brief Enumeration of the transform types
|
||
|
|
public: enum TransformType {TRANSLATE, ROTATE, SCALE, MATRIX};
|
||
|
|
|
||
|
|
/// \brief Constructor
|
||
|
|
/// \param[in] _type the type of transform
|
||
|
|
public: NodeTransform(TransformType _type = MATRIX);
|
||
|
|
|
||
|
|
/// \brief Constructor
|
||
|
|
/// \param[in] _mat the matrix
|
||
|
|
/// \param[in] _sid identifier
|
||
|
|
/// \param[in] _type the type of transform
|
||
|
|
public: NodeTransform(const ignition::math::Matrix4d &_mat,
|
||
|
|
const std::string &_sid = "_default_",
|
||
|
|
TransformType _type = MATRIX);
|
||
|
|
|
||
|
|
/// \brief Destructor. It does nothing.
|
||
|
|
public: ~NodeTransform();
|
||
|
|
|
||
|
|
/// \brief Assign a transformation
|
||
|
|
/// \param[in] _mat the transform
|
||
|
|
public: void Set(const ignition::math::Matrix4d &_mat);
|
||
|
|
|
||
|
|
/// \brief Set transform type
|
||
|
|
/// \param[in] _type the type
|
||
|
|
public: void SetType(TransformType _type);
|
||
|
|
|
||
|
|
/// \brief Set the SID
|
||
|
|
/// \param[in] _sid the sid
|
||
|
|
public: void SetSID(std::string _sid);
|
||
|
|
|
||
|
|
/// \brief Returns the transformation matrix
|
||
|
|
/// \return The transform matrix
|
||
|
|
public: ignition::math::Matrix4d GetTransform() const;
|
||
|
|
|
||
|
|
/// \brief Returns the transformation type
|
||
|
|
/// \return the type
|
||
|
|
public: TransformType GetType();
|
||
|
|
|
||
|
|
/// \brief Returns thr SID
|
||
|
|
/// \return the SID
|
||
|
|
public: std::string GetSID();
|
||
|
|
|
||
|
|
/// \brief Set a transformation matrix component value
|
||
|
|
/// \param[in] _idx the component index
|
||
|
|
/// \param[in] _value the value
|
||
|
|
public: void SetComponent(unsigned int _idx, double _value);
|
||
|
|
|
||
|
|
/// \brief Set source data values
|
||
|
|
/// param[in] _mat the values
|
||
|
|
public: void SetSourceValues(const ignition::math::Matrix4d &_mat);
|
||
|
|
|
||
|
|
/// \brief Set source data values
|
||
|
|
/// \param[in] _vec Vector to set source data values from.
|
||
|
|
public: void SetSourceValues(const ignition::math::Vector3d &_vec);
|
||
|
|
|
||
|
|
/// \brief Sets source matrix values from roation
|
||
|
|
/// \param[in] _axis of rotation
|
||
|
|
/// \param[in] _angle of rotation
|
||
|
|
public: void SetSourceValues(const ignition::math::Vector3d &_axis,
|
||
|
|
const double _angle);
|
||
|
|
|
||
|
|
/// \brief Sets the transform matrix from the source according to the type
|
||
|
|
public: void RecalculateMatrix();
|
||
|
|
|
||
|
|
/// \brief Prints the transform matrix to std::err stream
|
||
|
|
public: void PrintSource();
|
||
|
|
|
||
|
|
/// \brief Matrix cast operator
|
||
|
|
/// \return the transform
|
||
|
|
public: ignition::math::Matrix4d operator()();
|
||
|
|
|
||
|
|
/// \brief Node transform multiplication operator
|
||
|
|
/// \param[in] _t a transform
|
||
|
|
/// \return transform matrix multiplied by _t's transform
|
||
|
|
public: ignition::math::Matrix4d operator*(NodeTransform _t);
|
||
|
|
|
||
|
|
/// \brief Matrix multiplication operator
|
||
|
|
/// \param[in] _m a matrix
|
||
|
|
/// \return transform matrix multiplied by _m
|
||
|
|
public: ignition::math::Matrix4d operator*(
|
||
|
|
const ignition::math::Matrix4d &_m);
|
||
|
|
|
||
|
|
/// \brief the sid
|
||
|
|
protected: std::string sid;
|
||
|
|
|
||
|
|
/// \brief transform type
|
||
|
|
protected: TransformType type;
|
||
|
|
|
||
|
|
/// \brief transform
|
||
|
|
protected: ignition::math::Matrix4d transform;
|
||
|
|
|
||
|
|
/// \brief source data values (can be a matrix, a position or rotation)
|
||
|
|
protected: std::vector<double> source;
|
||
|
|
};
|
||
|
|
/// \}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|