Joints.hpp

Go to the documentation of this file.

/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */

#ifndef SIMULATION_JOINTS_TASK_HPP
#define SIMULATION_JOINTS_TASK_HPP

#include "mars/JointsBase.hpp"
#include <base/commands/Joints.hpp>

namespace mars {

    class Joints : public JointsBase
    {
        friend class JointsBase;
    protected:
        struct JointConversion
        {
            JointConversion()
                : mars_id(-1), scaling(1.0), offset(0.0), absolutePosition(0), lastPosition(0), gotPosition(false) {}

            double fromMars( double v )
            {
                return v * scaling + offset;
            }
            double toMars( double v )
            {
                return (v - offset) / scaling;
            }

            double updateAbsolutePosition( double v )
            {
                if(!gotPosition)
                {
                    gotPosition = true;
                    absolutePosition = v;
                    lastPosition = v;
                }
                
                double diff = v- lastPosition;
                
                if(diff > M_PI)
                {
                    diff -= 2* M_PI;
                }
                if(diff < -M_PI)
                {
                    diff += 2* M_PI;
                }
                
                absolutePosition += diff;
                
                lastPosition = v;
                
                return absolutePosition;
            }
            double getAbsolutePosition()
            {
                return absolutePosition;
            }
            
            int mars_id;
            std::string marsName;
            std::string externalName;
            double scaling;
            double offset;
            double absolutePosition;
            double lastPosition;
            bool gotPosition;
        };
        std::vector<JointConversion> mars_ids;
        enum JointTypes{MOTOR,PASSIVE};
        std::vector<JointTypes> joint_types;

        base::samples::Joints status;
        mars::JointCurrents currents;
        base::commands::Joints cmd;

        std::vector< mars::ParallelKinematic > parallel_kinematics;
        mars::JointPositionAndSpeedControlMode controlMode;

    public:
        virtual void init();
        virtual void update(double delta_t);

        Joints(std::string const& name = "mars::Joints");

        Joints(std::string const& name, RTT::ExecutionEngine* engine);

        ~Joints();

        bool configureHook();

        bool startHook();

        void updateHook();

        void errorHook();

        void stopHook();

        void cleanupHook();
    };
}

#endif