JointState.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_JOINTSTATE_HH_
18 #define GAZEBO_PHYSICS_JOINTSTATE_HH_
19 
20 #include <vector>
21 #include <string>
22 #include <ignition/math/Angle.hh>
23 
24 #include "gazebo/physics/State.hh"
25 #include "gazebo/util/system.hh"
26 
27 namespace gazebo
28 {
29  namespace physics
30  {
33 
36  class GZ_PHYSICS_VISIBLE JointState : public State
37  {
39  public: JointState();
40 
46  public: JointState(JointPtr _joint, const common::Time &_realTime,
47  const common::Time &_simTime, const uint64_t _iterations);
48 
51  public: explicit JointState(JointPtr _joint);
52 
57  public: explicit JointState(const sdf::ElementPtr _sdf);
58 
60  public: virtual ~JointState();
61 
66  public: void Load(JointPtr _joint, const common::Time &_realTime,
67  const common::Time &_simTime);
68 
71  public: virtual void Load(const sdf::ElementPtr _elem);
72 
75  public: unsigned int GetAngleCount() const;
76 
87  public: double Position(const unsigned int _axis = 0) const;
88 
95  public: const std::vector<double> &Positions() const;
96 
99  public: bool IsZero() const;
100 
103  public: void FillSDF(sdf::ElementPtr _sdf);
104 
108  public: JointState &operator=(const JointState &_state);
109 
113  public: JointState operator-(const JointState &_state) const;
114 
118  public: JointState operator+(const JointState &_state) const;
119 
124  public: inline friend std::ostream &operator<<(std::ostream &_out,
125  const gazebo::physics::JointState &_state)
126  {
127  if (_state.GetAngleCount() > 0)
128  {
129  _out << "<joint name='" << _state.GetName() << "'>";
130 
131  int i = 0;
132  for (std::vector<double>::const_iterator iter =
133  _state.positions.begin(); iter != _state.positions.end(); ++iter)
134  {
135  _out << "<angle axis='" << i << "'>" << (*iter) << "</angle>";
136  i++;
137  }
138 
139  _out << "</joint>";
140  }
141 
142  return _out;
143  }
144 
145  private: std::vector<double> positions;
146  };
148  }
149 }
150 #endif
Forward declarations for the common classes.
Definition: Animation.hh:26
boost::shared_ptr< Joint > JointPtr
Definition: PhysicsTypes.hh:118
State of an entity.
Definition: State.hh:43
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::JointState &_state)
Stream insertion operator.
Definition: JointState.hh:124
std::string GetName() const
Get the name associated with this State.
unsigned int GetAngleCount() const
Get the number of angles.
keeps track of state of a physics::Joint
Definition: JointState.hh:36
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:47