##

LinkState.hh
Go to the documentation of this file.
/*
* Copyright (C) 2012 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef GAZEBO_PHYSICS_LINKSTATE_HH_
#define GAZEBO_PHYSICS_LINKSTATE_HH_
#include <iomanip>
#include <iostream>
#include <vector>
#include <string>
#include <ignition/math/Pose3.hh>
#include <sdf/sdf.hh>
#include "gazebo/physics/State.hh"
#include "gazebo/physics/CollisionState.hh"
#include "gazebo/util/system.hh"
namespace gazebo
{
namespace physics
{
class GZ_PHYSICS_VISIBLE LinkState : public State
{
public: LinkState();
public: LinkState(const LinkPtr _link, const common::Time &_realTime,
const common::Time &_simTime, const uint64_t _iterations);
public: explicit LinkState(const LinkPtr _link);
public: explicit LinkState(const sdf::ElementPtr _sdf);
public: virtual ~LinkState();
public: void Load(const LinkPtr _link, const common::Time &_realTime,
const common::Time &_simTime, const uint64_t _iterations);
public: virtual void Load(const sdf::ElementPtr _elem);
public: const ignition::math::Pose3d &Pose() const;
public: const ignition::math::Pose3d &Velocity() const;
public: const ignition::math::Pose3d &Acceleration() const;
public: const ignition::math::Pose3d &Wrench() const;
public: unsigned int GetCollisionStateCount() const;
public: CollisionState GetCollisionState(unsigned int _index) const;
public: CollisionState GetCollisionState(
const std::string &_collisionName) const;
public: const std::vector<CollisionState> &GetCollisionStates() const;
public: bool IsZero() const;
public: void FillSDF(sdf::ElementPtr _sdf);
public: virtual void SetWallTime(const common::Time &_time);
public: virtual void SetRealTime(const common::Time &_time);
public: virtual void SetSimTime(const common::Time &_time);
public: virtual void SetIterations(const uint64_t _iterations);
public: LinkState &operator=(const LinkState &_state);
public: LinkState operator-(const LinkState &_state) const;
public: LinkState operator+(const LinkState &_state) const;
public: inline friend std::ostream &operator<<(std::ostream &_out,
const gazebo::physics::LinkState &_state)
{
ignition::math::Vector3d euler(_state.pose.Rot().Euler());
_out.unsetf(std::ios_base::floatfield);
_out << std::setprecision(4)
<< "<link name='" << _state.name << "'>"
<< "<pose>"
<< ignition::math::precision(_state.pose.Pos().X(), 4) << " "
<< ignition::math::precision(_state.pose.Pos().Y(), 4) << " "
<< ignition::math::precision(_state.pose.Pos().Z(), 4) << " "
<< ignition::math::precision(euler.X(), 4) << " "
<< ignition::math::precision(euler.Y(), 4) << " "
<< ignition::math::precision(euler.Z(), 4) << " "
<< "</pose>";
if (_state.RecordVelocity())
{
euler = _state.velocity.Rot().Euler();
_out.unsetf(std::ios_base::floatfield);
_out << std::setprecision(4)
<< "<velocity>"
<< ignition::math::precision(_state.velocity.Pos().X(), 4) << " "
<< ignition::math::precision(_state.velocity.Pos().Y(), 4) << " "
<< ignition::math::precision(_state.velocity.Pos().Z(), 4) << " "
<< ignition::math::precision(euler.X(), 4) << " "
<< ignition::math::precision(euler.Y(), 4) << " "
<< ignition::math::precision(euler.Z(), 4) << " "
<< "</velocity>";
}
// << "<acceleration>" << _state.acceleration << "</acceleration>"
// << "<wrench>" << _state.wrench << "</wrench>";
// for (std::vector<CollisionState>::const_iterator iter =
// _state.collisionStates.begin();
// iter != _state.collisionStates.end(); ++iter)
// {
// _out << *iter;
// }
_out << "</link>";
return _out;
}
public: void SetRecordVelocity(const bool _record);
public: bool RecordVelocity() const;
private: ignition::math::Pose3d pose;
private: ignition::math::Pose3d velocity;
private: ignition::math::Pose3d acceleration;
private: ignition::math::Pose3d wrench;
private: std::vector<CollisionState> collisionStates;
};
}
}
#endif
gazebo::physics::LinkState::operator+
LinkState operator+(const LinkState &_state) const
Addition operator.
gazebo::physics::LinkState::Wrench
const ignition::math::Pose3d & Wrench() const
Get the force and torque applied to the Link.
gazebo
Forward declarations for the common classes.
Definition: Animation.hh:26
CollisionState.hh
gazebo::physics::LinkState::SetRecordVelocity
void SetRecordVelocity(const bool _record)
Set whether to record link velocity.
gazebo::common::Time
A Time class, can be used to hold wall- or sim-time. stored as sec and nano-sec.
Definition: Time.hh:47
gazebo::physics::LinkState::Velocity
const ignition::math::Pose3d & Velocity() const
Get the link velocity.
gazebo::physics::LinkState::GetCollisionStateCount
unsigned int GetCollisionStateCount() const
Get the number of link states.
system.hh
gazebo::physics::LinkState::SetIterations
virtual void SetIterations(const uint64_t _iterations)
Set the simulation iterations when this state was generated.
gazebo::physics::LinkState::SetSimTime
virtual void SetSimTime(const common::Time &_time)
Set the sim time when this state was generated.
gazebo::physics::LinkState::LinkState
LinkState()
Default constructor.
gazebo::physics::LinkState
Store state information of a physics::Link object.
Definition: LinkState.hh:47
gazebo::physics::LinkState::RecordVelocity
bool RecordVelocity() const
Get whether link velocity is recorded.
gazebo::physics::LinkState::operator-
LinkState operator-(const LinkState &_state) const
Subtraction operator.
State.hh
gazebo::physics::LinkState::Acceleration
const ignition::math::Pose3d & Acceleration() const
Get the link acceleration.
gazebo::physics::LinkState::IsZero
bool IsZero() const
Return true if the values in the state are zero.
gazebo::physics::LinkState::SetWallTime
virtual void SetWallTime(const common::Time &_time)
Set the wall time when this state was generated.
gazebo::physics::LinkState::GetCollisionStates
const std::vector< CollisionState > & GetCollisionStates() const
Get the collision states.
gazebo::physics::LinkState::operator=
LinkState & operator=(const LinkState &_state)
Assignment operator.
gazebo::physics::CollisionState
Store state information of a physics::Collision object.
Definition: CollisionState.hh:43
gazebo::physics::LinkState::SetRealTime
virtual void SetRealTime(const common::Time &_time)
Set the real time when this state was generated.
gazebo::physics::LinkState::operator<<
friend std::ostream & operator<<(std::ostream &_out, const gazebo::physics::LinkState &_state)
Stream insertion operator.
Definition: LinkState.hh:186
gazebo::physics::LinkState::Load
void Load(const LinkPtr _link, const common::Time &_realTime, const common::Time &_simTime, const uint64_t _iterations)
Load a LinkState from a Link pointer.
gazebo::physics::State
State of an entity.
Definition: State.hh:43
gazebo::physics::LinkPtr
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:109
gazebo::physics::LinkState::Pose
const ignition::math::Pose3d & Pose() const
Get the link pose.
gazebo::physics::LinkState::FillSDF
void FillSDF(sdf::ElementPtr _sdf)
Populate a state SDF element with data from the object.
gazebo::physics::State::name
std::string name
Name associated with this State.
Definition: State.hh:124
gazebo::physics::LinkState::GetCollisionState
CollisionState GetCollisionState(unsigned int _index) const
Get a collision state.
gazebo::physics::LinkState::~LinkState
virtual ~LinkState()
Destructor.