LinkState.hh
Go to the documentation of this file.
1/*
2 * Copyright (C) 2012 Open Source Robotics Foundation
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 *
16*/
17#ifndef GAZEBO_PHYSICS_LINKSTATE_HH_
18#define GAZEBO_PHYSICS_LINKSTATE_HH_
19
20#include <iomanip>
21#include <iostream>
22#include <vector>
23#include <string>
24
25#include <ignition/math/Pose3.hh>
26#include <sdf/sdf.hh>
27
30#include "gazebo/util/system.hh"
31
32namespace gazebo
33{
34 namespace physics
35 {
38
47 class GZ_PHYSICS_VISIBLE LinkState : public State
48 {
50 public: LinkState();
51
60 public: LinkState(const LinkPtr _link, const common::Time &_realTime,
61 const common::Time &_simTime, const uint64_t _iterations);
62
68 public: explicit LinkState(const LinkPtr _link);
69
74 public: explicit LinkState(const sdf::ElementPtr _sdf);
75
77 public: virtual ~LinkState();
78
87 public: void Load(const LinkPtr _link, const common::Time &_realTime,
88 const common::Time &_simTime, const uint64_t _iterations);
89
94 public: virtual void Load(const sdf::ElementPtr _elem);
95
98 public: const ignition::math::Pose3d &Pose() const;
99
102 public: const ignition::math::Pose3d &Velocity() const;
103
106 public: const ignition::math::Pose3d &Acceleration() const;
107
110 public: const ignition::math::Pose3d &Wrench() const;
111
116 public: unsigned int GetCollisionStateCount() const;
117
125 public: CollisionState GetCollisionState(unsigned int _index) const;
126
135 const std::string &_collisionName) const;
136
139 public: const std::vector<CollisionState> &GetCollisionStates() const;
140
143 public: bool IsZero() const;
144
147 public: void FillSDF(sdf::ElementPtr _sdf);
148
152 public: virtual void SetWallTime(const common::Time &_time);
153
156 public: virtual void SetRealTime(const common::Time &_time);
157
160 public: virtual void SetSimTime(const common::Time &_time);
161
165 public: virtual void SetIterations(const uint64_t _iterations);
166
170 public: LinkState &operator=(const LinkState &_state);
171
175 public: LinkState operator-(const LinkState &_state) const;
176
180 public: LinkState operator+(const LinkState &_state) const;
181
186 public: inline friend std::ostream &operator<<(std::ostream &_out,
187 const gazebo::physics::LinkState &_state)
188 {
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 << "'>"
193 << "<pose>"
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) << " "
200 << "</pose>";
201
202 if (_state.RecordVelocity())
203 {
205 euler = _state.velocity.Rot().Euler();
206 _out.unsetf(std::ios_base::floatfield);
207 _out << std::setprecision(4)
208 << "<velocity>"
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) << " "
215 << "</velocity>";
216 }
217 // << "<acceleration>" << _state.acceleration << "</acceleration>"
218 // << "<wrench>" << _state.wrench << "</wrench>";
219
221 // for (std::vector<CollisionState>::const_iterator iter =
222 // _state.collisionStates.begin();
223 // iter != _state.collisionStates.end(); ++iter)
224 // {
225 // _out << *iter;
226 // }
227
228 _out << "</link>";
229
230 return _out;
231 }
232
236 public: void SetRecordVelocity(const bool _record);
237
240 public: bool RecordVelocity() const;
241
243 private: ignition::math::Pose3d pose;
244
247 private: ignition::math::Pose3d velocity;
248
251 private: ignition::math::Pose3d acceleration;
252
255 private: ignition::math::Pose3d wrench;
256
258 private: std::vector<CollisionState> collisionStates;
259 };
261 }
262}
263#endif
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