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#ifdef _WIN32
21 // Ensure that Winsock2.h is included before Windows.h, which can get
22 // pulled in by anybody (e.g., Boost).
23 #include <Winsock2.h>
24#endif
25
26#include <vector>
27#include <string>
28#include <ignition/math/Angle.hh>
29
31#include "gazebo/util/system.hh"
32
33namespace gazebo
34{
35 namespace physics
36 {
39
42 class GZ_PHYSICS_VISIBLE JointState : public State
43 {
45 public: JointState();
46
52 public: JointState(JointPtr _joint, const common::Time &_realTime,
53 const common::Time &_simTime, const uint64_t _iterations);
54
57 public: explicit JointState(JointPtr _joint);
58
63 public: explicit JointState(const sdf::ElementPtr _sdf);
64
66 public: virtual ~JointState();
67
72 public: void Load(JointPtr _joint, const common::Time &_realTime,
73 const common::Time &_simTime);
74
77 public: virtual void Load(const sdf::ElementPtr _elem);
78
81 public: unsigned int GetAngleCount() const;
82
93 public: double Position(const unsigned int _axis = 0) const;
94
101 public: const std::vector<double> &Positions() const;
102
105 public: bool IsZero() const;
106
109 public: void FillSDF(sdf::ElementPtr _sdf);
110
114 public: JointState &operator=(const JointState &_state);
115
119 public: JointState operator-(const JointState &_state) const;
120
124 public: JointState operator+(const JointState &_state) const;
125
130 public: inline friend std::ostream &operator<<(std::ostream &_out,
131 const gazebo::physics::JointState &_state)
132 {
133 if (_state.GetAngleCount() > 0)
134 {
135 _out << "<joint name='" << _state.GetName() << "'>";
136
137 int i = 0;
138 for (std::vector<double>::const_iterator iter =
139 _state.positions.begin(); iter != _state.positions.end(); ++iter)
140 {
141 _out << "<angle axis='" << i << "'>" << (*iter) << "</angle>";
142 i++;
143 }
144
145 _out << "</joint>";
146 }
147
148 return _out;
149 }
150
151 private: std::vector<double> positions;
152 };
154 }
155}
156#endif
A Time class, can be used to hold wall- or sim-time.
Definition: Time.hh:48
keeps track of state of a physics::Joint
Definition: JointState.hh:43
JointState(JointPtr _joint)
Constructor.
virtual ~JointState()
Destructor.
unsigned int GetAngleCount() const
Get the number of angles.
JointState operator+(const JointState &_state) const
Addition operator.
const std::vector< double > & Positions() const
Get the joint positions.
bool IsZero() const
Return true if the values in the state are zero.
JointState()
Default constructor.
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::JointState &_state)
Stream insertion operator.
Definition: JointState.hh:130
JointState & operator=(const JointState &_state)
Assignment operator.
double Position(const unsigned int _axis=0) const
Get the joint position.
JointState operator-(const JointState &_state) const
Subtraction operator.
void Load(JointPtr _joint, const common::Time &_realTime, const common::Time &_simTime)
Load.
void FillSDF(sdf::ElementPtr _sdf)
Populate a state SDF element with data from the object.
virtual void Load(const sdf::ElementPtr _elem)
Load state from SDF element.
JointState(const sdf::ElementPtr _sdf)
Constructor.
JointState(JointPtr _joint, const common::Time &_realTime, const common::Time &_simTime, const uint64_t _iterations)
Constructor.
State of an entity.
Definition: State.hh:50
std::string GetName() const
Get the name associated with this State.
boost::shared_ptr< Joint > JointPtr
Definition: PhysicsTypes.hh:117
Forward declarations for the common classes.
Definition: Animation.hh:27