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 
28 #include "gazebo/physics/State.hh"
30 #include "gazebo/util/system.hh"
31 
32 namespace 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 
134  public: CollisionState GetCollisionState(
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
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:109
std::string name
Name associated with this State.
Definition: State.hh:130
bool RecordVelocity() const
Get whether link velocity is recorded.
Forward declarations for the common classes.
Definition: Animation.hh:26
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:186
Store state information of a physics::Collision object.
Definition: CollisionState.hh:43
Store state information of a physics::Link object.
Definition: LinkState.hh:47
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44