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 #include "gazebo/util/system.hh"
33 
34 namespace gazebo
35 {
36  namespace physics
37  {
40 
50  {
52  public: LinkState();
53 
61  public: LinkState(const LinkPtr _link, const common::Time &_realTime,
62  const common::Time &_simTime);
63 
69  public: explicit LinkState(const LinkPtr _link);
70 
75  public: explicit LinkState(const sdf::ElementPtr _sdf);
76 
78  public: virtual ~LinkState();
79 
87  public: void Load(const LinkPtr _link, const common::Time &_realTime,
88  const common::Time &_simTime);
89 
94  public: virtual void Load(const sdf::ElementPtr _elem);
95 
98  public: const math::Pose &GetPose() const;
99 
102  public: const math::Pose &GetVelocity() const;
103 
106  public: const math::Pose &GetAcceleration() const;
107 
110  public: const math::Pose &GetWrench() 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: LinkState &operator=(const LinkState &_state);
166 
170  public: LinkState operator-(const LinkState &_state) const;
171 
175  public: LinkState operator+(const LinkState &_state) const;
176 
181  public: inline friend std::ostream &operator<<(std::ostream &_out,
182  const gazebo::physics::LinkState &_state)
183  {
184  math::Vector3 q(_state.pose.rot.GetAsEuler());
185  _out << std::fixed <<std::setprecision(5)
186  << "<link name='" << _state.name << "'>"
187  << "<pose>"
188  << _state.pose.pos.x << " "
189  << _state.pose.pos.y << " "
190  << _state.pose.pos.z << " "
191  << q.x << " "
192  << q.y << " "
193  << q.z << " "
194  << "</pose>";
195 
197  q = _state.velocity.rot.GetAsEuler();
198  _out << std::fixed <<std::setprecision(4)
199  << "<velocity>"
200  << _state.velocity.pos.x << " "
201  << _state.velocity.pos.y << " "
202  << _state.velocity.pos.z << " "
203  << q.x << " "
204  << q.y << " "
205  << q.z << " "
206  << "</velocity>";
207  // << "<acceleration>" << _state.acceleration << "</acceleration>"
208  // << "<wrench>" << _state.wrench << "</wrench>";
209 
211  // for (std::vector<CollisionState>::const_iterator iter =
212  // _state.collisionStates.begin();
213  // iter != _state.collisionStates.end(); ++iter)
214  // {
215  // _out << *iter;
216  // }
217 
218  _out << "</link>";
219 
220  return _out;
221  }
222 
224  private: math::Pose pose;
225 
227  private: math::Pose velocity;
228 
230  private: math::Pose acceleration;
231 
233  private: math::Pose wrench;
234 
236  private: std::vector<CollisionState> collisionStates;
237  };
239  }
240 }
241 #endif