LinkState.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2016 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.
 *
*/
/* Desc: A link state
 * Author: Nate Koenig
 */

#ifndef _LINKSTATE_HH_
#define _LINKSTATE_HH_

#include <vector>
#include <string>

#include <sdf/sdf.hh>

#include "gazebo/physics/State.hh"
#include "gazebo/physics/CollisionState.hh"
#include "gazebo/math/Pose.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 math::Pose &GetPose() const;

      public: const math::Pose &GetVelocity() const;

      public: const math::Pose &GetAcceleration() const;

      public: const math::Pose &GetWrench() 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)
      {
        math::Vector3 q(_state.pose.rot.GetAsEuler());
        _out << std::fixed <<std::setprecision(5)
          << "<link name='" << _state.name << "'>"
          << "<pose>"
          << _state.pose.pos.x << " "
          << _state.pose.pos.y << " "
          << _state.pose.pos.z << " "
          << q.x << " "
          << q.y << " "
          << q.z << " "
          << "</pose>";

        q = _state.velocity.rot.GetAsEuler();
         _out << std::fixed <<std::setprecision(4)
           << "<velocity>"
           << _state.velocity.pos.x << " "
           << _state.velocity.pos.y << " "
           << _state.velocity.pos.z << " "
           << q.x << " "
           << q.y << " "
           << q.z << " "
           << "</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;
      }

      private: math::Pose pose;

      private: math::Pose velocity;

      private: math::Pose acceleration;

      private: math::Pose wrench;

      private: std::vector<CollisionState> collisionStates;
    };
  }
}
#endif