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 /* Desc: A link state
18  * Author: Nate Koenig
19  */
20 
21 #ifndef _LINKSTATE_HH_
22 #define _LINKSTATE_HH_
23 
24 #include <iostream>
25 #include <vector>
26 #include <string>
27 
28 #include <sdf/sdf.hh>
29 
30 #include "gazebo/physics/State.hh"
32 #include "gazebo/math/Pose.hh"
33 #include "gazebo/util/system.hh"
34 
35 namespace gazebo
36 {
37  namespace physics
38  {
41 
50  class GZ_PHYSICS_VISIBLE LinkState : public State
51  {
53  public: LinkState();
54 
63  public: LinkState(const LinkPtr _link, const common::Time &_realTime,
64  const common::Time &_simTime, const uint64_t _iterations);
65 
71  public: explicit LinkState(const LinkPtr _link);
72 
77  public: explicit LinkState(const sdf::ElementPtr _sdf);
78 
80  public: virtual ~LinkState();
81 
90  public: void Load(const LinkPtr _link, const common::Time &_realTime,
91  const common::Time &_simTime, const uint64_t _iterations);
92 
97  public: virtual void Load(const sdf::ElementPtr _elem);
98 
101  public: const math::Pose &GetPose() const;
102 
105  public: const math::Pose &GetVelocity() const;
106 
109  public: const math::Pose &GetAcceleration() const;
110 
113  public: const math::Pose &GetWrench() const;
114 
119  public: unsigned int GetCollisionStateCount() const;
120 
128  public: CollisionState GetCollisionState(unsigned int _index) const;
129 
137  public: CollisionState GetCollisionState(
138  const std::string &_collisionName) const;
139 
142  public: const std::vector<CollisionState> &GetCollisionStates() const;
143 
146  public: bool IsZero() const;
147 
150  public: void FillSDF(sdf::ElementPtr _sdf);
151 
155  public: virtual void SetWallTime(const common::Time &_time);
156 
159  public: virtual void SetRealTime(const common::Time &_time);
160 
163  public: virtual void SetSimTime(const common::Time &_time);
164 
168  public: virtual void SetIterations(const uint64_t _iterations);
169 
173  public: LinkState &operator=(const LinkState &_state);
174 
178  public: LinkState operator-(const LinkState &_state) const;
179 
183  public: LinkState operator+(const LinkState &_state) const;
184 
189  public: inline friend std::ostream &operator<<(std::ostream &_out,
190  const gazebo::physics::LinkState &_state)
191  {
192  math::Vector3 q(_state.pose.rot.GetAsEuler());
193  _out.unsetf(std::ios_base::floatfield);
194  _out << std::setprecision(4)
195  << "<link name='" << _state.name << "'>"
196  << "<pose>"
197  << ignition::math::precision(_state.pose.pos.x, 4) << " "
198  << ignition::math::precision(_state.pose.pos.y, 4) << " "
199  << ignition::math::precision(_state.pose.pos.z, 4) << " "
200  << ignition::math::precision(q.x, 4) << " "
201  << ignition::math::precision(q.y, 4) << " "
202  << ignition::math::precision(q.z, 4) << " "
203  << "</pose>";
204 
205  if (_state.RecordVelocity())
206  {
208  q = _state.velocity.rot.GetAsEuler();
209  _out.unsetf(std::ios_base::floatfield);
210  _out << std::setprecision(4)
211  << "<velocity>"
212  << ignition::math::precision(_state.velocity.pos.x, 4) << " "
213  << ignition::math::precision(_state.velocity.pos.y, 4) << " "
214  << ignition::math::precision(_state.velocity.pos.z, 4) << " "
215  << ignition::math::precision(q.x, 4) << " "
216  << ignition::math::precision(q.y, 4) << " "
217  << ignition::math::precision(q.z, 4) << " "
218  << "</velocity>";
219  }
220  // << "<acceleration>" << _state.acceleration << "</acceleration>"
221  // << "<wrench>" << _state.wrench << "</wrench>";
222 
224  // for (std::vector<CollisionState>::const_iterator iter =
225  // _state.collisionStates.begin();
226  // iter != _state.collisionStates.end(); ++iter)
227  // {
228  // _out << *iter;
229  // }
230 
231  _out << "</link>";
232 
233  return _out;
234  }
235 
239  public: void SetRecordVelocity(const bool _record);
240 
243  public: bool RecordVelocity() const;
244 
246  private: math::Pose pose;
247 
249  private: math::Pose velocity;
250 
252  private: math::Pose acceleration;
253 
255  private: math::Pose wrench;
256 
258  private: std::vector<CollisionState> collisionStates;
259  };
261  }
262 }
263 #endif
double x
X location.
Definition: Vector3.hh:311
Quaternion rot
The rotation.
Definition: Pose.hh:255
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:100
std::string name
Name associated with this State.
Definition: State.hh:130
double y
Y location.
Definition: Vector3.hh:314
bool RecordVelocity() const
Get whether link velocity is recorded.
Forward declarations for the common classes.
Definition: Animation.hh:33
Encapsulates a position and rotation in three space.
Definition: Pose.hh:37
The Vector3 class represents the generic vector containing 3 elements.
Definition: Vector3.hh:39
Vector3 GetAsEuler() const
Return the rotation in Euler angles.
State of an entity.
Definition: State.hh:49
double z
Z location.
Definition: Vector3.hh:317
Vector3 pos
The position.
Definition: Pose.hh:252
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::LinkState &_state)
Stream insertion operator.
Definition: LinkState.hh:189
T precision(const T &_a, const unsigned int &_precision)
get value at a specified precision
Definition: Helpers.hh:182
Store state information of a physics::Collision object.
Definition: CollisionState.hh:42
Store state information of a physics::Link object.
Definition: LinkState.hh:50
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44