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 <vector>
21 #include <string>
22 
23 #include <ignition/math/Pose3.hh>
24 #include <sdf/sdf.hh>
25 
26 #include "gazebo/physics/State.hh"
28 #include "gazebo/math/Pose.hh"
29 #include "gazebo/util/system.hh"
30 
31 namespace gazebo
32 {
33  namespace physics
34  {
37 
46  class GZ_PHYSICS_VISIBLE LinkState : public State
47  {
49  public: LinkState();
50 
59  public: LinkState(const LinkPtr _link, const common::Time &_realTime,
60  const common::Time &_simTime, const uint64_t _iterations);
61 
67  public: explicit LinkState(const LinkPtr _link);
68 
73  public: explicit LinkState(const sdf::ElementPtr _sdf);
74 
76  public: virtual ~LinkState();
77 
86  public: void Load(const LinkPtr _link, const common::Time &_realTime,
87  const common::Time &_simTime, const uint64_t _iterations);
88 
93  public: virtual void Load(const sdf::ElementPtr _elem);
94 
98  public: const math::Pose GetPose() const GAZEBO_DEPRECATED(8.0);
99 
102  public: const ignition::math::Pose3d &Pose() const;
103 
107  public: const math::Pose GetVelocity() const GAZEBO_DEPRECATED(8.0);
108 
111  public: const ignition::math::Pose3d &Velocity() const;
112 
116  public: const math::Pose GetAcceleration() const GAZEBO_DEPRECATED(8.0);
117 
120  public: const ignition::math::Pose3d &Acceleration() const;
121 
125  public: const math::Pose GetWrench() const GAZEBO_DEPRECATED(8.0);
126 
129  public: const ignition::math::Pose3d &Wrench() const;
130 
135  public: unsigned int GetCollisionStateCount() const;
136 
144  public: CollisionState GetCollisionState(unsigned int _index) const;
145 
153  public: CollisionState GetCollisionState(
154  const std::string &_collisionName) const;
155 
158  public: const std::vector<CollisionState> &GetCollisionStates() const;
159 
162  public: bool IsZero() const;
163 
166  public: void FillSDF(sdf::ElementPtr _sdf);
167 
171  public: virtual void SetWallTime(const common::Time &_time);
172 
175  public: virtual void SetRealTime(const common::Time &_time);
176 
179  public: virtual void SetSimTime(const common::Time &_time);
180 
184  public: virtual void SetIterations(const uint64_t _iterations);
185 
189  public: LinkState &operator=(const LinkState &_state);
190 
194  public: LinkState operator-(const LinkState &_state) const;
195 
199  public: LinkState operator+(const LinkState &_state) const;
200 
205  public: inline friend std::ostream &operator<<(std::ostream &_out,
206  const gazebo::physics::LinkState &_state)
207  {
208  ignition::math::Vector3d euler(_state.pose.Rot().Euler());
209  _out << std::fixed <<std::setprecision(5)
210  << "<link name='" << _state.name << "'>"
211  << "<pose>"
212  << _state.pose.Pos().X() << " "
213  << _state.pose.Pos().Y() << " "
214  << _state.pose.Pos().Z() << " "
215  << euler.X() << " "
216  << euler.Y() << " "
217  << euler.Z() << " "
218  << "</pose>";
219 
221  euler = _state.velocity.Rot().Euler();
222  _out << std::fixed <<std::setprecision(4)
223  << "<velocity>"
224  << _state.velocity.Pos().X() << " "
225  << _state.velocity.Pos().Y() << " "
226  << _state.velocity.Pos().Z() << " "
227  << euler.X() << " "
228  << euler.Y() << " "
229  << euler.Z() << " "
230  << "</velocity>";
231  // << "<acceleration>" << _state.acceleration << "</acceleration>"
232  // << "<wrench>" << _state.wrench << "</wrench>";
233 
235  // for (std::vector<CollisionState>::const_iterator iter =
236  // _state.collisionStates.begin();
237  // iter != _state.collisionStates.end(); ++iter)
238  // {
239  // _out << *iter;
240  // }
241 
242  _out << "</link>";
243 
244  return _out;
245  }
246 
248  private: ignition::math::Pose3d pose;
249 
251  private: ignition::math::Pose3d velocity;
252 
254  private: ignition::math::Pose3d acceleration;
255 
257  private: ignition::math::Pose3d wrench;
258 
260  private: std::vector<CollisionState> collisionStates;
261  };
263  }
264 }
265 #endif
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:109
std::string name
Name associated with this State.
Definition: State.hh:130
Forward declarations for the common classes.
Definition: Animation.hh:33
Encapsulates a position and rotation in three space.
Definition: Pose.hh:42
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:205
#define GAZEBO_DEPRECATED(version)
Definition: system.hh:302
Store state information of a physics::Collision object.
Definition: CollisionState.hh:44
Store state information of a physics::Link object.
Definition: LinkState.hh:46
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:44