Joint.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_JOINT_HH_
#define GAZEBO_PHYSICS_JOINT_HH_
 
#include <string>
#include <vector>
 
#include <boost/any.hpp>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>
 
#include "gazebo/common/Event.hh"
#include "gazebo/common/Events.hh"
#include "gazebo/msgs/MessageTypes.hh"
 
#include "gazebo/physics/JointState.hh"
#include "gazebo/physics/Base.hh"
#include "gazebo/physics/JointWrench.hh"
#include "gazebo/util/system.hh"
 
#define MAX_JOINT_AXIS 2
 
namespace gazebo
{
  namespace physics
  {
 
    class GZ_PHYSICS_VISIBLE Joint : public Base
    {
      public: enum Attribute
              {
                FUDGE_FACTOR,
 
                SUSPENSION_ERP,
 
                SUSPENSION_CFM,
 
                STOP_ERP,
 
                STOP_CFM,
 
                ERP,
 
                CFM,
 
                FMAX,
 
                VEL,
 
                HI_STOP,
 
                LO_STOP
              };
 
      public: explicit Joint(BasePtr _parent);
 
      public: virtual ~Joint();
 
      public: void Load(LinkPtr _parent, LinkPtr _child,
                        const ignition::math::Pose3d &_pose);
 
      public: virtual void Load(sdf::ElementPtr _sdf) override;
 
      public: virtual void Init() override;
 
      public: virtual void Fini() override;
 
      public: void Update() override;
 
      public: virtual void UpdateParameters(sdf::ElementPtr _sdf) override;
 
      public: virtual void Reset() override;
      using Base::Reset;
 
      public: void SetState(const JointState &_state);
 
      public: void SetModel(ModelPtr _model);
 
      public: virtual LinkPtr GetJointLink(unsigned int _index) const = 0;
 
      public: virtual bool AreConnected(LinkPtr _one, LinkPtr _two) const = 0;
 
      public: virtual void Attach(LinkPtr _parent, LinkPtr _child);
 
      public: virtual void Detach();
 
      public: virtual void SetAxis(const unsigned int _index,
                                   const ignition::math::Vector3d &_axis) = 0;
 
      public: virtual void SetDamping(unsigned int _index, double _damping) = 0;
 
      public: double GetDamping(unsigned int _index);
 
      public: virtual void ApplyStiffnessDamping();
 
      public: virtual void SetStiffnessDamping(unsigned int _index,
        double _stiffness, double _damping, double _reference = 0) = 0;
 
      public: virtual void SetStiffness(unsigned int _index,
                                        const double _stiffness) = 0;
 
      public: double GetStiffness(unsigned int _index);
 
      public: double GetSpringReferencePosition(unsigned int _index) const;
 
      public: template<typename T>
              event::ConnectionPtr ConnectJointUpdate(T _subscriber)
              {return jointUpdate.Connect(_subscriber);}
 
      public: ignition::math::Vector3d LocalAxis(const unsigned int _index)
          const;
 
      public: virtual ignition::math::Vector3d GlobalAxis(
                  unsigned int _index) const = 0;
 
      public: virtual void SetAnchor(const unsigned int _index,
          const ignition::math::Vector3d &_anchor) = 0;
 
      public: virtual ignition::math::Vector3d Anchor(
          const unsigned int _index) const = 0;
 
      public: virtual double GetEffortLimit(unsigned int _index);
 
      public: virtual void SetEffortLimit(unsigned int _index, double _effort);
 
      public: virtual double GetVelocityLimit(unsigned int _index);
 
      public: virtual void SetVelocityLimit(unsigned int _index,
                                                  double _velocity);
 
      public: virtual void SetVelocity(unsigned int _index, double _vel) = 0;
 
      public: virtual double GetVelocity(unsigned int _index) const = 0;
 
      public: virtual void SetForce(unsigned int _index, double _effort) = 0;
 
      public: double CheckAndTruncateForce(unsigned int _index, double _effort);
 
      public: virtual double GetForce(unsigned int _index);
 
      public: virtual JointWrench GetForceTorque(unsigned int _index) = 0;
 
      public: virtual double Position(const unsigned int _index = 0) const
          final;
 
      public: virtual unsigned int DOF() const = 0;
 
      public: virtual bool SetPosition(
                  const unsigned int _index, const double _position,
                  const bool _preserveWorldVelocity = false);
 
      protected: bool SetPositionMaximal(
                  const unsigned int _index, double _position,
                  const bool _preserveWorldVelocity = false);
 
      protected: bool SetVelocityMaximal(unsigned int _index, double _velocity);
 
      public: virtual ignition::math::Vector3d LinkForce(
          const unsigned int _index) const = 0;
 
      public: virtual ignition::math::Vector3d LinkTorque(
          const unsigned int _index) const = 0;
 
      public: virtual bool SetParam(const std::string &_key,
                                    unsigned int _index,
                                    const boost::any &_value) = 0;
 
      public: virtual double GetParam(const std::string &_key,
                                      unsigned int _index);
 
      public: LinkPtr GetChild() const;
 
      public: LinkPtr GetParent() const;
 
      public: msgs::Joint::Type GetMsgType() const;
 
      public: virtual void FillMsg(msgs::Joint &_msg);
 
      public: double GetInertiaRatio(const unsigned int _index) const;
 
      public: double InertiaRatio(const ignition::math::Vector3d &_axis) const;
 
      public: virtual double LowerLimit(unsigned int _index = 0) const;
 
      public: virtual double UpperLimit(const unsigned int _index = 0) const;
 
      public: virtual void SetLowerLimit(const unsigned int _index,
                                         const double _limit);
 
      public: virtual void SetUpperLimit(const unsigned int _index,
                                         const double _limit);
 
      public: virtual void SetProvideFeedback(bool _enable);
 
      public: virtual void CacheForceTorque();
 
      public: void SetStopStiffness(unsigned int _index, double _stiffness);
 
      public: void SetStopDissipation(unsigned int _index, double _dissipation);
 
      public: double GetStopStiffness(unsigned int _index) const;
 
      public: double GetStopDissipation(unsigned int _index) const;
 
      public: ignition::math::Pose3d InitialAnchorPose() const;
 
      public: ignition::math::Pose3d WorldPose() const;
 
      public: ignition::math::Pose3d ParentWorldPose() const;
 
      public: ignition::math::Pose3d AnchorErrorPose() const;
 
      public: ignition::math::Vector3d ResolveAxisXyz(
          const unsigned int _index,
          const std::string &_resolveTo = "") const;
 
      public: ignition::math::Quaterniond AxisFrame(
          const unsigned int _index) const;
 
      public: ignition::math::Quaterniond AxisFrameOffset(
          const unsigned int _index) const;
 
      public: double GetWorldEnergyPotentialSpring(unsigned int _index) const;
 
      // Documentation inherited
      public: virtual std::optional<sdf::SemanticPose> SDFSemanticPose()
                  const override;
 
      public: const sdf::Joint *GetSDFDom() const;
 
      protected: virtual double PositionImpl(const unsigned int _index = 0)
          const = 0;
 
      protected: bool FindAllConnectedLinks(const LinkPtr &_originalParentLink,
        Link_V &_connectedLinks);
 
      protected: ignition::math::Pose3d ChildLinkPose(
          const unsigned int _index, const double _position);
 
      protected: virtual void RegisterIntrospectionItems() override;
 
      private: void RegisterIntrospectionPosition(const unsigned int _index);
 
      private: void RegisterIntrospectionVelocity(const unsigned int _index);
 
      private: void LoadImpl(const ignition::math::Pose3d &_pose);
 
      protected: LinkPtr childLink;
 
      protected: LinkPtr parentLink;
 
      protected: ModelPtr model;
 
      protected: ignition::math::Vector3d anchorPos;
 
      protected: ignition::math::Pose3d anchorPose;
 
      protected: ignition::math::Pose3d parentAnchorPose;
 
      protected: LinkPtr anchorLink;
 
      protected: double dissipationCoefficient[MAX_JOINT_AXIS];
 
      protected: double stiffnessCoefficient[MAX_JOINT_AXIS];
 
      protected: double springReferencePosition[MAX_JOINT_AXIS];
 
      protected: gazebo::event::ConnectionPtr applyDamping;
 
      protected: double effortLimit[MAX_JOINT_AXIS];
 
      protected: double velocityLimit[MAX_JOINT_AXIS];
 
      protected: double lowerLimit[MAX_JOINT_AXIS];
 
      protected: double upperLimit[MAX_JOINT_AXIS];
 
      protected: JointWrench wrench;
 
      protected: std::string axisExpressedIn[MAX_JOINT_AXIS];
 
      private: static sdf::ElementPtr sdfJoint;
 
      protected: bool provideFeedback;
 
      private: std::vector<std::string> sensors;
 
      private: event::EventT<void ()> jointUpdate;
 
      private: double staticPosition;
 
      private: double stopStiffness[MAX_JOINT_AXIS];
 
      private: double stopDissipation[MAX_JOINT_AXIS];
 
      private: const sdf::Joint *jointSDFDom = nullptr;
    };
  }
}
#endif