All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
LinkState.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 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 <vector>
25 #include <string>
26 
27 #include <sdf/sdf.hh>
28 
29 #include "gazebo/physics/State.hh"
31 #include "gazebo/math/Pose.hh"
32 
33 namespace gazebo
34 {
35  namespace physics
36  {
39 
48  class LinkState : public State
49  {
51  public: LinkState();
52 
60  public: LinkState(const LinkPtr _link, const common::Time &_realTime,
61  const common::Time &_simTime);
62 
68  public: explicit LinkState(const LinkPtr _link);
69 
74  public: explicit LinkState(const sdf::ElementPtr _sdf);
75 
77  public: virtual ~LinkState();
78 
86  public: void Load(const LinkPtr _link, const common::Time &_realTime,
87  const common::Time &_simTime);
88 
93  public: virtual void Load(const sdf::ElementPtr _elem);
94 
97  public: const math::Pose &GetPose() const;
98 
101  public: const math::Pose &GetVelocity() const;
102 
105  public: const math::Pose &GetAcceleration() const;
106 
109  public: const math::Pose &GetWrench() const;
110 
115  public: unsigned int GetCollisionStateCount() const;
116 
124  public: CollisionState GetCollisionState(unsigned int _index) const;
125 
134  const std::string &_collisionName) const;
135 
138  public: const std::vector<CollisionState> &GetCollisionStates() const;
139 
142  public: bool IsZero() const;
143 
146  public: void FillSDF(sdf::ElementPtr _sdf);
147 
151  public: virtual void SetWallTime(const common::Time &_time);
152 
155  public: virtual void SetRealTime(const common::Time &_time);
156 
159  public: virtual void SetSimTime(const common::Time &_time);
160 
164  public: LinkState &operator=(const LinkState &_state);
165 
169  public: LinkState operator-(const LinkState &_state) const;
170 
174  public: LinkState operator+(const LinkState &_state) const;
175 
180  public: inline friend std::ostream &operator<<(std::ostream &_out,
181  const gazebo::physics::LinkState &_state)
182  {
183  math::Vector3 q(_state.pose.rot.GetAsEuler());
184  _out << std::fixed <<std::setprecision(5)
185  << "<link name='" << _state.name << "'>"
186  << "<pose>"
187  << _state.pose.pos.x << " "
188  << _state.pose.pos.y << " "
189  << _state.pose.pos.z << " "
190  << q.x << " "
191  << q.y << " "
192  << q.z << " "
193  << "</pose>";
194 
196  q = _state.velocity.rot.GetAsEuler();
197  _out << std::fixed <<std::setprecision(4)
198  << "<velocity>"
199  << _state.velocity.pos.x << " "
200  << _state.velocity.pos.y << " "
201  << _state.velocity.pos.z << " "
202  << q.x << " "
203  << q.y << " "
204  << q.z << " "
205  << "</velocity>";
206  // << "<acceleration>" << _state.acceleration << "</acceleration>"
207  // << "<wrench>" << _state.wrench << "</wrench>";
208 
210  // for (std::vector<CollisionState>::const_iterator iter =
211  // _state.collisionStates.begin();
212  // iter != _state.collisionStates.end(); ++iter)
213  // {
214  // _out << *iter;
215  // }
216 
217  _out << "</link>";
218 
219  return _out;
220  }
221 
223  private: math::Pose pose;
224 
226  private: math::Pose velocity;
227 
229  private: math::Pose acceleration;
230 
232  private: math::Pose wrench;
233 
235  private: std::vector<CollisionState> collisionStates;
236  };
238  }
239 }
240 #endif