LinkState.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2016 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 
49  class GZ_PHYSICS_VISIBLE LinkState : public State
50  {
52  public: LinkState();
53 
62  public: LinkState(const LinkPtr _link, const common::Time &_realTime,
63  const common::Time &_simTime, const uint64_t _iterations);
64 
70  public: explicit LinkState(const LinkPtr _link);
71 
76  public: explicit LinkState(const sdf::ElementPtr _sdf);
77 
79  public: virtual ~LinkState();
80 
89  public: void Load(const LinkPtr _link, const common::Time &_realTime,
90  const common::Time &_simTime, const uint64_t _iterations);
91 
96  public: virtual void Load(const sdf::ElementPtr _elem);
97 
100  public: const math::Pose &GetPose() const;
101 
104  public: const math::Pose &GetVelocity() const;
105 
108  public: const math::Pose &GetAcceleration() const;
109 
112  public: const math::Pose &GetWrench() const;
113 
118  public: unsigned int GetCollisionStateCount() const;
119 
127  public: CollisionState GetCollisionState(unsigned int _index) const;
128 
136  public: CollisionState GetCollisionState(
137  const std::string &_collisionName) const;
138 
141  public: const std::vector<CollisionState> &GetCollisionStates() const;
142 
145  public: bool IsZero() const;
146 
149  public: void FillSDF(sdf::ElementPtr _sdf);
150 
154  public: virtual void SetWallTime(const common::Time &_time);
155 
158  public: virtual void SetRealTime(const common::Time &_time);
159 
162  public: virtual void SetSimTime(const common::Time &_time);
163 
167  public: virtual void SetIterations(const uint64_t _iterations);
168 
172  public: LinkState &operator=(const LinkState &_state);
173 
177  public: LinkState operator-(const LinkState &_state) const;
178 
182  public: LinkState operator+(const LinkState &_state) const;
183 
188  public: inline friend std::ostream &operator<<(std::ostream &_out,
189  const gazebo::physics::LinkState &_state)
190  {
191  math::Vector3 q(_state.pose.rot.GetAsEuler());
192  _out << std::fixed <<std::setprecision(5)
193  << "<link name='" << _state.name << "'>"
194  << "<pose>"
195  << _state.pose.pos.x << " "
196  << _state.pose.pos.y << " "
197  << _state.pose.pos.z << " "
198  << q.x << " "
199  << q.y << " "
200  << q.z << " "
201  << "</pose>";
202 
204  q = _state.velocity.rot.GetAsEuler();
205  _out << std::fixed <<std::setprecision(4)
206  << "<velocity>"
207  << _state.velocity.pos.x << " "
208  << _state.velocity.pos.y << " "
209  << _state.velocity.pos.z << " "
210  << q.x << " "
211  << q.y << " "
212  << q.z << " "
213  << "</velocity>";
214  // << "<acceleration>" << _state.acceleration << "</acceleration>"
215  // << "<wrench>" << _state.wrench << "</wrench>";
216 
218  // for (std::vector<CollisionState>::const_iterator iter =
219  // _state.collisionStates.begin();
220  // iter != _state.collisionStates.end(); ++iter)
221  // {
222  // _out << *iter;
223  // }
224 
225  _out << "</link>";
226 
227  return _out;
228  }
229 
231  private: math::Pose pose;
232 
234  private: math::Pose velocity;
235 
237  private: math::Pose acceleration;
238 
240  private: math::Pose wrench;
241 
243  private: std::vector<CollisionState> collisionStates;
244  };
246  }
247 }
248 #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
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:188
Store state information of a physics::Collision object.
Definition: CollisionState.hh:42
Store state information of a physics::Link object.
Definition: LinkState.hh:49
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44