307 lines
11 KiB
C++
307 lines
11 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_MODELSTATE_HH_
|
|
#define _GAZEBO_MODELSTATE_HH_
|
|
|
|
#include <vector>
|
|
#include <string>
|
|
#include <boost/regex.hpp>
|
|
|
|
#include <ignition/math/Vector3.hh>
|
|
#include "gazebo/math/Pose.hh"
|
|
|
|
#include "gazebo/physics/State.hh"
|
|
#include "gazebo/physics/LinkState.hh"
|
|
#include "gazebo/physics/JointState.hh"
|
|
#include "gazebo/util/system.hh"
|
|
|
|
namespace gazebo
|
|
{
|
|
namespace physics
|
|
{
|
|
/// \addtogroup gazebo_physics
|
|
/// \{
|
|
|
|
/// \class ModelState ModelState.hh physics/physics.hh
|
|
/// \brief Store state information of a physics::Model object
|
|
///
|
|
/// This class captures the entire state of a Model at one
|
|
/// specific time during a simulation run.
|
|
///
|
|
/// State of a Model includes the state of all its child Links and
|
|
/// Joints.
|
|
class GZ_PHYSICS_VISIBLE ModelState : public State
|
|
{
|
|
/// \brief Default constructor.
|
|
public: ModelState();
|
|
|
|
/// \brief Constructor.
|
|
///
|
|
/// Build a ModelState from an existing Model.
|
|
/// \param[in] _model Pointer to the model from which to gather state
|
|
/// info.
|
|
/// \param[in] _realTime Real time stamp.
|
|
/// \param[in] _simTime Sim time stamp.
|
|
/// \param[in] _iterations Simulation iterations.
|
|
public: ModelState(const ModelPtr _model, const common::Time &_realTime,
|
|
const common::Time &_simTime, const uint64_t _iterations);
|
|
|
|
/// \brief Constructor.
|
|
///
|
|
/// Build a ModelState from an existing Model.
|
|
/// \param[in] _model Pointer to the model from which to gather state
|
|
/// info.
|
|
public: explicit ModelState(const ModelPtr _model);
|
|
|
|
/// \brief Constructor
|
|
///
|
|
/// Build a ModelState from SDF data
|
|
/// \param[in] _sdf SDF data to load a model state from.
|
|
public: explicit ModelState(const sdf::ElementPtr _sdf);
|
|
|
|
/// \brief Destructor.
|
|
public: virtual ~ModelState();
|
|
|
|
/// \brief Load state from Model pointer.
|
|
///
|
|
/// Build a ModelState from an existing Model.
|
|
/// \param[in] _model Pointer to the model from which to gather state
|
|
/// info.
|
|
/// \param[in] _realTime Real time stamp.
|
|
/// \param[in] _simTime Sim time stamp.
|
|
/// \param[in] _iterations Simulation iterations.
|
|
public: void Load(const ModelPtr _model, const common::Time &_realTime,
|
|
const common::Time &_simTime, const uint64_t _iterations);
|
|
|
|
/// \brief Load state from SDF element.
|
|
///
|
|
/// Load ModelState information from stored data in and SDF::Element
|
|
/// \param[in] _elem Pointer to the SDF::Element containing state info.
|
|
public: virtual void Load(const sdf::ElementPtr _elem);
|
|
|
|
/// \brief Get the stored model pose.
|
|
/// \return The math::Pose of the Model.
|
|
public: const math::Pose &GetPose() const;
|
|
|
|
/// \brief Get the stored model scale.
|
|
/// \return The scale of the Model.
|
|
public: const ignition::math::Vector3d &Scale() const;
|
|
|
|
/// \brief Return true if the values in the state are zero.
|
|
/// \return True if the values in the state are zero.
|
|
public: bool IsZero() const;
|
|
|
|
/// \brief Get the number of link states.
|
|
///
|
|
/// This returns the number of Links recorded.
|
|
/// \return Number of LinkState recorded.
|
|
public: unsigned int GetLinkStateCount() const;
|
|
|
|
/// \brief Get link states based on a regular expression.
|
|
/// \param[in] _regex The regular expression.
|
|
/// \return List of link states whose names match the regular
|
|
/// expression.
|
|
public: LinkState_M GetLinkStates(const boost::regex &_regex) const;
|
|
|
|
/// \brief Get joint states based on a regular expression.
|
|
/// \param[in] _regex The regular expression.
|
|
/// \return List of joint states whose names match the regular
|
|
/// expression.
|
|
public: JointState_M GetJointStates(const boost::regex &_regex) const;
|
|
|
|
/// \brief Get a link state by Link name
|
|
///
|
|
/// Searches through all LinkStates. Returns the LinkState with the
|
|
/// matching name, if any.
|
|
/// \param[in] _linkName Name of the LinkState
|
|
/// \return State of the Link.
|
|
/// \throws common::Exception When _linkName is invalid.
|
|
public: LinkState GetLinkState(const std::string &_linkName) const;
|
|
|
|
/// \brief Return true if there is a link with the specified name.
|
|
/// \param[in] _linkName Name of the LinkState.
|
|
/// \return True if the link exists in the model.
|
|
public: bool HasLinkState(const std::string &_linkName) const;
|
|
|
|
/// \brief Get the link states.
|
|
/// \return A map of link states.
|
|
public: const LinkState_M &GetLinkStates() const;
|
|
|
|
/// \brief Get the number of joint states.
|
|
///
|
|
/// Returns the number of JointStates recorded.
|
|
/// \return Number of JointStates.
|
|
public: unsigned int GetJointStateCount() const;
|
|
|
|
/// \brief Get a Joint state.
|
|
///
|
|
/// Return a JointState based on a index, where index is between
|
|
/// 0...ModelState::GetJointStateCount().
|
|
/// \param[in] _index Index of a JointState.
|
|
/// \return State of a Joint.
|
|
/// \throws common::Exception When _index is out of range.
|
|
public: JointState GetJointState(unsigned int _index) const;
|
|
|
|
/// \brief Get a Joint state by Joint name.
|
|
///
|
|
/// Searches through all JointStates. Returns the JointState with the
|
|
/// matching name, if any.
|
|
/// \param[in] _jointName Name of the JointState.
|
|
/// \return State of the Joint.
|
|
/// \throws common::Exception When _jointName is invalid.
|
|
public: JointState GetJointState(const std::string &_jointName) const;
|
|
|
|
/// \brief Get the joint states.
|
|
/// \return A map of joint states.
|
|
public: const JointState_M &GetJointStates() const;
|
|
|
|
/// \brief Return true if there is a joint with the specified name.
|
|
/// \param[in] _jointName Name of the Jointtate.
|
|
/// \return True if the joint exists in the model.
|
|
public: bool HasJointState(const std::string &_jointName) const;
|
|
|
|
/// \brief Get the number of model states.
|
|
///
|
|
/// This returns the number of nested model states recorded.
|
|
/// \return Number of nested ModelState recorded.
|
|
public: unsigned int NestedModelStateCount() const;
|
|
|
|
/// \brief Get a model state by model name
|
|
///
|
|
/// Searches through all nested model states. Returns the model state with
|
|
/// the matching name, if any.
|
|
/// \param[in] _modelName Name of the model state
|
|
/// \return State of the Model.
|
|
/// \throws common::Exception When _modelName is invalid.
|
|
public: ModelState NestedModelState(const std::string &_modelName) const;
|
|
|
|
/// \brief Return true if there is a nested model with the specified name.
|
|
/// \param[in] _modelName Name of the model state.
|
|
/// \return True if the model exists in this model state.
|
|
public: bool HasNestedModelState(const std::string &_modelName) const;
|
|
|
|
/// \brief Get the nested model states.
|
|
/// \return A map of model names to model states.
|
|
public: const ModelState_M &NestedModelStates() const;
|
|
|
|
/// \brief Populate a state SDF element with data from the object.
|
|
/// \param[out] _sdf SDF element to populate.
|
|
public: void FillSDF(sdf::ElementPtr _sdf);
|
|
|
|
/// \brief Set the wall time when this state was generated
|
|
/// \param[in] _time The absolute clock time when the State
|
|
/// data was recorded.
|
|
public: virtual void SetWallTime(const common::Time &_time);
|
|
|
|
/// \brief Set the real time when this state was generated
|
|
/// \param[in] _time Clock time since simulation was stated.
|
|
public: virtual void SetRealTime(const common::Time &_time);
|
|
|
|
/// \brief Set the sim time when this state was generated
|
|
/// \param[in] _time Simulation time when the data was recorded.
|
|
public: virtual void SetSimTime(const common::Time &_time);
|
|
|
|
/// \brief Set the simulation iterations when this state was generated
|
|
/// \param[in] _iterations Simulation iterations when the data was
|
|
/// recorded.
|
|
public: virtual void SetIterations(const uint64_t _iterations);
|
|
|
|
/// \brief Assignment operator
|
|
/// \param[in] _state State value
|
|
/// \return this
|
|
public: ModelState &operator=(const ModelState &_state);
|
|
|
|
/// \brief Subtraction operator.
|
|
/// \param[in] _pt A state to substract.
|
|
/// \return The resulting state.
|
|
public: ModelState operator-(const ModelState &_state) const;
|
|
|
|
/// \brief Addition operator.
|
|
/// \param[in] _pt A state to substract.
|
|
/// \return The resulting state.
|
|
public: ModelState operator+(const ModelState &_state) const;
|
|
|
|
/// \brief Stream insertion operator.
|
|
/// \param[in] _out output stream.
|
|
/// \param[in] _state Model state to output.
|
|
/// \return The stream.
|
|
public: inline friend std::ostream &operator<<(std::ostream &_out,
|
|
const gazebo::physics::ModelState &_state)
|
|
{
|
|
math::Vector3 q(_state.pose.rot.GetAsEuler());
|
|
_out.unsetf(std::ios_base::floatfield);
|
|
_out << std::setprecision(3)
|
|
<< "<model name='" << _state.GetName() << "'>"
|
|
<< "<pose>"
|
|
<< ignition::math::precision(_state.pose.pos.x, 4) << " "
|
|
<< ignition::math::precision(_state.pose.pos.y, 4) << " "
|
|
<< ignition::math::precision(_state.pose.pos.z, 4) << " "
|
|
<< ignition::math::precision(q.x, 4) << " "
|
|
<< ignition::math::precision(q.y, 4) << " "
|
|
<< ignition::math::precision(q.z, 4) << " "
|
|
<< "</pose>";
|
|
|
|
// Only record scale if it is not the default value of [1, 1, 1].
|
|
if (_state.scale != ignition::math::Vector3d::One)
|
|
_out << "<scale>" << _state.scale << "</scale>";
|
|
|
|
for (LinkState_M::const_iterator iter =
|
|
_state.linkStates.begin(); iter != _state.linkStates.end();
|
|
++iter)
|
|
{
|
|
_out << iter->second;
|
|
}
|
|
|
|
for (const auto &ms : _state.modelStates)
|
|
{
|
|
_out << ms.second;
|
|
}
|
|
|
|
// Output the joint information
|
|
// for (JointState_M::const_iterator iter =
|
|
// _state.jointStates.begin(); iter != _state.jointStates.end();
|
|
// ++iter)
|
|
// {
|
|
// _out << iter->second;
|
|
// }
|
|
|
|
_out << "</model>";
|
|
|
|
return _out;
|
|
}
|
|
|
|
/// \brief Pose of the model.
|
|
private: math::Pose pose;
|
|
|
|
/// \brief Scale of the model.
|
|
private: ignition::math::Vector3d scale;
|
|
|
|
/// \brief All the link states.
|
|
private: LinkState_M linkStates;
|
|
|
|
/// \brief All the joint states.
|
|
private: JointState_M jointStates;
|
|
|
|
/// \brief All the model states.
|
|
private: ModelState_M modelStates;
|
|
};
|
|
/// \}
|
|
}
|
|
}
|
|
#endif
|