17#ifndef GAZEBO_PHYSICS_LINKSTATE_HH_
18#define GAZEBO_PHYSICS_LINKSTATE_HH_
25#include <ignition/math/Pose3.hh>
61 const common::Time &_simTime,
const uint64_t _iterations);
74 public:
explicit LinkState(
const sdf::ElementPtr _sdf);
88 const common::Time &_simTime,
const uint64_t _iterations);
94 public:
virtual void Load(
const sdf::ElementPtr _elem);
98 public:
const ignition::math::Pose3d &
Pose()
const;
102 public:
const ignition::math::Pose3d &
Velocity()
const;
110 public:
const ignition::math::Pose3d &
Wrench()
const;
135 const std::string &_collisionName)
const;
186 public:
inline friend std::ostream &
operator<<(std::ostream &_out,
189 ignition::math::Vector3d euler(_state.pose.Rot().Euler());
190 _out.unsetf(std::ios_base::floatfield);
191 _out << std::setprecision(4)
192 <<
"<link name='" << _state.
name <<
"'>"
194 << ignition::math::precision(_state.pose.Pos().X(), 4) <<
" "
195 << ignition::math::precision(_state.pose.Pos().Y(), 4) <<
" "
196 << ignition::math::precision(_state.pose.Pos().Z(), 4) <<
" "
197 << ignition::math::precision(euler.X(), 4) <<
" "
198 << ignition::math::precision(euler.Y(), 4) <<
" "
199 << ignition::math::precision(euler.Z(), 4) <<
" "
205 euler = _state.velocity.Rot().Euler();
206 _out.unsetf(std::ios_base::floatfield);
207 _out << std::setprecision(4)
209 << ignition::math::precision(_state.velocity.Pos().X(), 4) <<
" "
210 << ignition::math::precision(_state.velocity.Pos().Y(), 4) <<
" "
211 << ignition::math::precision(_state.velocity.Pos().Z(), 4) <<
" "
212 << ignition::math::precision(euler.X(), 4) <<
" "
213 << ignition::math::precision(euler.Y(), 4) <<
" "
214 << ignition::math::precision(euler.Z(), 4) <<
" "
243 private: ignition::math::Pose3d pose;
247 private: ignition::math::Pose3d velocity;
251 private: ignition::math::Pose3d acceleration;
255 private: ignition::math::Pose3d wrench;
258 private: std::vector<CollisionState> collisionStates;
A Time class, can be used to hold wall- or sim-time.
Definition Time.hh:48
Store state information of a physics::Collision object.
Definition CollisionState.hh:44
Store state information of a physics::Link object.
Definition LinkState.hh:48
virtual void SetRealTime(const common::Time &_time)
Set the real time when this state was generated.
virtual void SetSimTime(const common::Time &_time)
Set the sim time when this state was generated.
LinkState operator+(const LinkState &_state) const
Addition operator.
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::LinkState &_state)
Stream insertion operator.
Definition LinkState.hh:186
const ignition::math::Pose3d & Acceleration() const
Get the link acceleration.
virtual ~LinkState()
Destructor.
virtual void SetWallTime(const common::Time &_time)
Set the wall time when this state was generated.
LinkState(const LinkPtr _link, const common::Time &_realTime, const common::Time &_simTime, const uint64_t _iterations)
Constructor.
const ignition::math::Pose3d & Pose() const
Get the link pose.
const std::vector< CollisionState > & GetCollisionStates() const
Get the collision states.
bool IsZero() const
Return true if the values in the state are zero.
LinkState operator-(const LinkState &_state) const
Subtraction operator.
LinkState(const sdf::ElementPtr _sdf)
Constructor.
void SetRecordVelocity(const bool _record)
Set whether to record link velocity.
LinkState & operator=(const LinkState &_state)
Assignment operator.
LinkState()
Default constructor.
unsigned int GetCollisionStateCount() const
Get the number of link states.
CollisionState GetCollisionState(const std::string &_collisionName) const
Get a link state by link name.
const ignition::math::Pose3d & Wrench() const
Get the force and torque applied to the Link.
void FillSDF(sdf::ElementPtr _sdf)
Populate a state SDF element with data from the object.
virtual void Load(const sdf::ElementPtr _elem)
Load state from SDF element.
LinkState(const LinkPtr _link)
Constructor.
virtual void SetIterations(const uint64_t _iterations)
Set the simulation iterations when this state was generated.
bool RecordVelocity() const
Get whether link velocity is recorded.
const ignition::math::Pose3d & Velocity() const
Get the link velocity.
CollisionState GetCollisionState(unsigned int _index) const
Get a collision state.
void Load(const LinkPtr _link, const common::Time &_realTime, const common::Time &_simTime, const uint64_t _iterations)
Load a LinkState from a Link pointer.
State of an entity.
Definition State.hh:44
std::string name
Name associated with this State.
Definition State.hh:124
boost::shared_ptr< Link > LinkPtr
Definition PhysicsTypes.hh:109
Forward declarations for the common classes.
Definition Animation.hh:27