17 #ifndef GAZEBO_PHYSICS_LINKSTATE_HH_ 18 #define GAZEBO_PHYSICS_LINKSTATE_HH_ 23 #include <ignition/math/Pose3.hh> 60 const common::Time &_simTime,
const uint64_t _iterations);
73 public:
explicit LinkState(
const sdf::ElementPtr _sdf);
87 const common::Time &_simTime,
const uint64_t _iterations);
93 public:
virtual void Load(
const sdf::ElementPtr _elem);
102 public:
const ignition::math::Pose3d &Pose()
const;
111 public:
const ignition::math::Pose3d &Velocity()
const;
120 public:
const ignition::math::Pose3d &Acceleration()
const;
129 public:
const ignition::math::Pose3d &Wrench()
const;
135 public:
unsigned int GetCollisionStateCount()
const;
144 public:
CollisionState GetCollisionState(
unsigned int _index)
const;
154 const std::string &_collisionName)
const;
158 public:
const std::vector<CollisionState> &GetCollisionStates()
const;
162 public:
bool IsZero()
const;
166 public:
void FillSDF(sdf::ElementPtr _sdf);
171 public:
virtual void SetWallTime(
const common::Time &_time);
175 public:
virtual void SetRealTime(
const common::Time &_time);
179 public:
virtual void SetSimTime(
const common::Time &_time);
184 public:
virtual void SetIterations(
const uint64_t _iterations);
205 public:
inline friend std::ostream &
operator<<(std::ostream &_out,
208 ignition::math::Vector3d euler(_state.pose.Rot().Euler());
209 _out << std::fixed <<std::setprecision(5)
210 <<
"<link name='" << _state.
name <<
"'>" 212 << _state.pose.Pos().X() <<
" " 213 << _state.pose.Pos().Y() <<
" " 214 << _state.pose.Pos().Z() <<
" " 221 euler = _state.velocity.Rot().Euler();
222 _out << std::fixed <<std::setprecision(4)
224 << _state.velocity.Pos().X() <<
" " 225 << _state.velocity.Pos().Y() <<
" " 226 << _state.velocity.Pos().Z() <<
" " 248 private: ignition::math::Pose3d pose;
251 private: ignition::math::Pose3d velocity;
254 private: ignition::math::Pose3d acceleration;
257 private: ignition::math::Pose3d wrench;
260 private: std::vector<CollisionState> collisionStates;
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:109
std::string name
Name associated with this State.
Definition: State.hh:130
Forward declarations for the common classes.
Definition: Animation.hh:33
Encapsulates a position and rotation in three space.
Definition: Pose.hh:42
State of an entity.
Definition: State.hh:49
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::LinkState &_state)
Stream insertion operator.
Definition: LinkState.hh:205
#define GAZEBO_DEPRECATED(version)
Definition: system.hh:302
Store state information of a physics::Collision object.
Definition: CollisionState.hh:44
Store state information of a physics::Link object.
Definition: LinkState.hh:46
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44