HAL_LegTarget.hpp

Go to the documentation of this file.

 
#ifndef HalLegTarget_DEFINED
#define HalLegTarget_DEFINED
 
#include "wb_hal_legtarget.h"
//#include <cmath>
#include <sstream>
 
//#ifndef M_PIF
//#define M_PIF static_cast<float>(M_PI)
//#endif  // M_PIF
 
namespace guWhiteboard
{
    class HalLegTarget : public wb_hal_legtarget
    {
 
        public:
//            HalLegTarget(const uint8_t &target_leg = LEFT_LEG): wb_hal_legtarget(target_leg) {}
        
            HalLegTarget(const uint8_t &target_leg = LEFT_LEG,
                         const float hipyawpitch = 0,
                         const float hiproll = 0,
                         const float hippitch = 0,
                         const float kneepitch = 0,
                         const float anklepitch = 0,
                         const float ankleroll = 0,
                         const float hipyawpitchstiffness = 0,
                         const float hiprollstiffness = 0,
                         const float hippitchstiffness = 0,
                         const float kneepitchstiffness = 0,
                         const float anklepitchstiffness = 0,
                         const float anklerollstiffness = 0,
                         const bool  hipyawpitch_active = true,
                         const bool  hiproll_active = true,
                         const bool  hippitch_active = true,
                         const bool  kneepitch_active = true,
                         const bool  anklepitch_active = true,
                         const bool  ankleroll_active = true,
                         const int32_t movement_time = 1000000,
                         const uint8_t pliability = 10,
                         const bool  leg_stop = false,
                         const bool inRadians = false)
            : wb_hal_legtarget(target_leg)
            {
               if (inRadians) {
                   setPose_Rad(hipyawpitch, hiproll,
                               hippitch, kneepitch,
                               anklepitch, ankleroll);
               } else {
                   setPose_Deg(hipyawpitch, hiproll,
                               hippitch, kneepitch,
                               anklepitch, ankleroll);
               }
               set_target_movement_time(movement_time);
               set_target_leg_stop(leg_stop);
               set_hipyawpitchstiffness(hipyawpitchstiffness);
               set_hiprollstiffness(hiprollstiffness);
               set_hippitchstiffness(hippitchstiffness);
               set_kneepitchstiffness(kneepitchstiffness);
               set_anklepitchstiffness(anklepitchstiffness);
               set_anklerollstiffness(anklerollstiffness);
               setLegPliability(hipyawpitch_active, hiproll_active, hippitch_active,
                                kneepitch_active, anklepitch_active, ankleroll_active);
               set_target_pliability(pliability);
            }
 
 
//MARK: Leg - General
            void tieToLeg(uint8_t leg)
            {
                set_target_leg(leg);
            }
 
//MARK: Leg - Movements
 
            void goToWithTime_Rad(float hipyawpitch,
                                  float hiproll,
                                  float hippitch,
                                  float kneepitch,
                                  float anklepitch,
                                  float ankleroll,
                                  int32_t time = INT_MAX)
            {
                setPose_Rad(hipyawpitch, hiproll,
                            hippitch, kneepitch,
                            anklepitch, ankleroll);
                set_target_movement_time(time);
            }
            
            void goToWithTime_Deg(float hipyawpitch,
                                  float hiproll,
                                  float hippitch,
                                  float kneepitch,
                                  float anklepitch,
                                  float ankleroll,
                                  int32_t time = INT_MAX)
            {
                setPose_Deg(hipyawpitch, hiproll,
                            hippitch, kneepitch,
                            anklepitch, ankleroll);
                set_target_movement_time(time);
            }
        
            void stop() {
                set_target_leg_stop(true);
            }
 
        
            void ready() {
                set_target_leg_stop(false);
            }
 
        
            void isAtGoal(bool goalReached)
            {
                set_target_leg_at_goal(goalReached);
            }
            
            bool atGoal()
            {
                return target_leg_at_goal();
            }
 
            bool atTargetLocation(HalLegTarget status, HalLegTarget tolerance)
            {
                int16_t hipyawpitchMargin = static_cast<int16_t>(abs(target_hipyawpitch() - status.target_hipyawpitch()));
                int16_t hiprollMargin = static_cast<int16_t>(abs(target_hiproll() - status.target_hiproll()));
                int16_t hippitchMargin = static_cast<int16_t>(abs(target_hippitch() - status.target_hippitch()));
                int16_t kneepitchMargin = static_cast<int16_t>(abs(target_kneepitch() - status.target_kneepitch()));
                int16_t anklepitchMargin = static_cast<int16_t>(abs(target_anklepitch() - status.target_anklepitch()));
                int16_t anklerollMargin = static_cast<int16_t>(abs(target_ankleroll() - status.target_ankleroll()));
                if (   (hipyawpitchMargin <= tolerance.target_hipyawpitch())
                    && (hiprollMargin <= tolerance.target_hiproll())
                    && (hippitchMargin <= tolerance.target_hippitch())
                    && (kneepitchMargin <= tolerance.target_kneepitch())
                    && (anklepitchMargin <= tolerance.target_anklepitch())
                    && (anklerollMargin <= tolerance.target_ankleroll())
                    )
                {
                    return true;
                }
                return false;
            }
 
//MARK: Leg - Pose
 
            void setPose_Rad(float hipyawpitch,
                             float hiproll,
                             float hippitch,
                             float kneepitch,
                             float anklepitch,
                             float ankleroll)
            {
                set_hipyawpitch_RAD(hipyawpitch);
                set_hiproll_RAD(hiproll);
                set_hippitch_RAD(hippitch);
                set_kneepitch_RAD(kneepitch);
                set_anklepitch_RAD(anklepitch);
                set_ankleroll_RAD(ankleroll);
            }
            
            void setPose_Deg(float hipyawpitch,
                             float hiproll,
                             float hippitch,
                             float kneepitch,
                             float anklepitch,
                             float ankleroll)
            {
                set_hipyawpitch_DEG(hipyawpitch);
                set_hiproll_DEG(hiproll);
                set_hippitch_DEG(hippitch);
                set_kneepitch_DEG(kneepitch);
                set_anklepitch_DEG(anklepitch);
                set_ankleroll_DEG(ankleroll);
            }
 
            
            void mirrorLeg(const HalLegTarget &other)
            {
                // Roll and Yaw angles need to be mirrored, others just copied.
                set_target_hipyawpitch(other.target_hipyawpitch());
                set_target_hiproll(-other.target_hiproll());
                set_target_hippitch(other.target_hippitch());
                set_target_kneepitch(other.target_kneepitch());
                set_target_anklepitch(other.target_anklepitch());
                set_target_ankleroll(-other.target_ankleroll());
                set_target_hipyawpitchstiffness(other.target_hipyawpitchstiffness());
                set_target_hiprollstiffness(other.target_hiprollstiffness());
                set_target_hippitchstiffness(other.target_hippitchstiffness());
                set_target_kneepitchstiffness(other.target_kneepitchstiffness());
                set_target_anklepitchstiffness(other.target_anklepitchstiffness());
                set_target_anklerollstiffness(other.target_anklerollstiffness());
                set_target_hipyawpitch_active(other.target_hipyawpitch_active());
                set_target_hiproll_active(other.target_hiproll_active());
                set_target_hippitch_active(other.target_hippitch_active());
                set_target_kneepitch_active(other.target_kneepitch_active());
                set_target_anklepitch_active(other.target_anklepitch_active());
                set_target_ankleroll_active(other.target_ankleroll_active());
                set_target_movement_time(other.target_movement_time());
                set_target_pliability(other.target_pliability());
            }
            
            void copyPose(const HalLegTarget &other)
            {
                set_target_hipyawpitch(other.target_hipyawpitch());
                set_target_hiproll(other.target_hiproll());
                set_target_hippitch(other.target_hippitch());
                set_target_kneepitch(other.target_kneepitch());
                set_target_anklepitch(other.target_anklepitch());
                set_target_ankleroll(other.target_ankleroll());
            }
            
            void mirrorPose(const HalLegTarget &other)
            {
                // Roll and Yaw angles need to be mirrored, others just copied.
                set_target_hipyawpitch(-other.target_hipyawpitch());
                set_target_hiproll(-other.target_hiproll());
                set_target_hippitch(other.target_hippitch());
                set_target_kneepitch(other.target_kneepitch());
                set_target_anklepitch(other.target_anklepitch());
                set_target_ankleroll(-other.target_ankleroll());
            }
            
            bool hasSamePose(const HalLegTarget &other)
            {
                if (
                    target_hipyawpitch() == other.target_hipyawpitch()
                    && target_hiproll() == other.target_hiproll()
                    && target_hippitch() == other.target_hippitch()
                    && target_kneepitch() == other.target_kneepitch()
                    && target_anklepitch() == other.target_anklepitch()
                    && target_ankleroll() == other.target_ankleroll()
                    )
                {
                    return  true;
                }
                return false;
            }
            
            bool hasSameMirroredPose(const HalLegTarget &other)
            {
                if (
                    target_hipyawpitch() == other.target_hipyawpitch()
                    && target_hiproll() == -other.target_hiproll()
                    && target_hippitch() == other.target_hippitch()
                    && target_kneepitch() == other.target_kneepitch()
                    && target_anklepitch() == other.target_anklepitch()
                    && target_ankleroll() == -other.target_ankleroll()
                    )
                {
                    return  true;
                }
                return false;
            }
 
//MARK: Leg - Stiffness
 
            void setLegStiffnessMax()
            {
                set_hipyawpitchstiffness(1.0f);
                set_hiprollstiffness(1.0f);
                set_hippitchstiffness(1.0f);
                set_kneepitchstiffness(1.0f);
                set_anklepitchstiffness(1.0f);
                set_anklerollstiffness(1.0f);
            }
 
            void setLegStiffnessNormal()
            {
                set_hipyawpitchstiffness(0.6f);
                set_hiprollstiffness(0.6f);
                set_hippitchstiffness(0.6f);
                set_kneepitchstiffness(0.6f);
                set_anklepitchstiffness(0.6f);
                set_anklerollstiffness(0.6f);
            }
 
            void setLegStiffness(float stiffness)
            {
                if ((stiffness <= 1.0f) && (stiffness >=0.0f)) {
                    set_hipyawpitchstiffness(stiffness);
                    set_hiprollstiffness(stiffness);
                    set_hippitchstiffness(stiffness);
                    set_kneepitchstiffness(stiffness);
                    set_anklepitchstiffness(stiffness);
                    set_anklerollstiffness(stiffness);
                }
            }
 
            void setLegStiffnessOff()
            {
                set_hipyawpitchstiffness(0.0f);
                set_hiprollstiffness(0.0f);
                set_hippitchstiffness(0.0f);
                set_kneepitchstiffness(0.0f);
                set_anklepitchstiffness(0.0f);
                set_anklerollstiffness(0.0f);
            }
 
        
            void copyStiffness(const HalLegTarget &other)
            {
                target_hipyawpitchstiffness() = other.target_hipyawpitchstiffness();
                target_hiprollstiffness() = other.target_hiprollstiffness();
                target_hippitchstiffness() = other.target_hippitchstiffness();
                target_kneepitchstiffness() = other.target_kneepitchstiffness();
                target_anklepitchstiffness() = other.target_anklepitchstiffness();
                target_anklerollstiffness() = other.target_anklerollstiffness();
            }
        
 
 
            bool hasSameStiffness(const HalLegTarget &other)
            {
                if (
                    target_hipyawpitchstiffness() == other.target_hipyawpitchstiffness()
                    && target_hiprollstiffness() == other.target_hiprollstiffness()
                    && target_hippitchstiffness() == other.target_hippitchstiffness()
                    && target_kneepitchstiffness() == other.target_kneepitchstiffness()
                    && target_anklepitchstiffness() == other.target_anklepitchstiffness()
                    && target_anklerollstiffness() == other.target_anklerollstiffness()
                   )
                {
                    return  true;
                }
                return false;
            }
 
//MARK: Leg - Pliability
                
            void setLegActive()
            {
                set_target_hipyawpitch_active(true);
                set_target_hiproll_active(true);
                set_target_hippitch_active(true);
                set_target_kneepitch_active(true);
                set_target_anklepitch_active(true);
                set_target_ankleroll_active(true);
            }
 
            void setLegPassive()
            {
                set_target_hipyawpitch_active(false);
                set_target_hiproll_active(false);
                set_target_hippitch_active(false);
                set_target_kneepitch_active(false);
                set_target_anklepitch_active(false);
                set_target_ankleroll_active(false);
            }
            
            
            void setLegPliability(bool hipyawpitch, bool hiproll, bool hippitch,
                                bool kneepitch, bool anklepitch, bool ankleroll)
            {
                set_target_hipyawpitch_active(hipyawpitch);
                set_target_hiproll_active(hiproll);
                set_target_hippitch_active(hippitch);
                set_target_kneepitch_active(kneepitch);
                set_target_anklepitch_active(anklepitch);
                set_target_ankleroll_active(ankleroll);
            }
 
            bool isLegPassive() {
                return !target_hipyawpitch_active() || !target_hiproll_active() || !target_hippitch_active() || !target_kneepitch_active() || !target_anklepitch_active() || !target_ankleroll_active();
            }
 
            bool isLegAllPassive() {
                return !target_hipyawpitch_active() && !target_hiproll_active() && !target_hippitch_active() && !target_kneepitch_active() && !target_anklepitch_active() && !target_ankleroll_active();
            }
        
            bool isLegAllActive() {
                return target_hipyawpitch_active() && target_hiproll_active() && target_hippitch_active() && target_kneepitch_active() && target_anklepitch_active() && target_ankleroll_active();
            }
        
//MARK: CUSTOM SETTERS (Converting floats into Integer representations)
            void set_hipyawpitch_DEG(float setting) {
                set_target_hipyawpitch(static_cast<int16_t>(setting * 10.0f));
            }
            
            void set_hiproll_DEG(float setting) {
                set_target_hiproll(static_cast<int16_t>(setting * 10.0f));
            }
        
            void set_hippitch_DEG(float setting) {
                set_target_hippitch(static_cast<int16_t>(setting * 10.0f));
            }
        
            void set_kneepitch_DEG(float setting) {
                set_target_kneepitch(static_cast<int16_t>(setting * 10.0f));
            }
            
            void set_anklepitch_DEG(float setting) {
                set_target_anklepitch(static_cast<int16_t>(setting * 10.0f));
            }
 
            void set_ankleroll_DEG(float setting) {
                set_target_ankleroll(static_cast<int16_t>(setting * 10.0f));
            }
 
 
            void set_hipyawpitch_RAD(float setting) {
                set_target_hipyawpitch(static_cast<int16_t>(setting* DEG_OVER_RAD_10));
            }
            
            void set_hiproll_RAD(float setting) {
                set_target_hiproll(static_cast<int16_t>(setting * DEG_OVER_RAD_10));
            }
        
            void set_hippitch_RAD(float setting) {
                set_target_hippitch(static_cast<int16_t>(setting * DEG_OVER_RAD_10));
            }
        
            void set_kneepitch_RAD(float setting) {
                set_target_kneepitch(static_cast<int16_t>(setting * DEG_OVER_RAD_10));
            }
            
            void set_anklepitch_RAD(float setting) {
                set_target_anklepitch(static_cast<int16_t>(setting * DEG_OVER_RAD_10));
            }
 
            void set_ankleroll_RAD(float setting) {
                set_target_ankleroll(static_cast<int16_t>(setting * DEG_OVER_RAD_10));
            }
 
            void set_hipyawpitchstiffness(float setting) {
                set_target_hipyawpitchstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
            
            void set_hiprollstiffness(float setting) {
                set_target_hiprollstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
        
            void set_hippitchstiffness(float setting) {
                set_target_hippitchstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
        
            void set_kneepitchstiffness(float setting) {
                set_target_kneepitchstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
            
            void set_anklepitchstiffness(float setting) {
                set_target_anklepitchstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
 
            void set_anklerollstiffness(float setting) {
                set_target_anklerollstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
 
 
//            void set_movement_time(int32_t time) {
//                set_target_movement_time(time);
//            }
        
//MARK: CUSTOM GETTERS (Converting Integer representations back to floats)
            float get_hipyawpitch_DEG() {
                return static_cast<float>(target_hipyawpitch() * 0.1f);
            }
            
            float get_hiproll_DEG() {
                return static_cast<float>(target_hiproll() * 0.1f);
            }
        
            float get_hippitch_DEG() {
                return static_cast<float>(target_hippitch() * 0.1f);
            }
        
            float get_kneepitch_DEG() {
                return static_cast<float>(target_kneepitch() * 0.1f);
            }
            
            float get_anklepitch_DEG() {
                return static_cast<float>(target_anklepitch() * 0.1f);
            }
 
            float get_ankleroll_DEG() {
                return static_cast<float>(target_ankleroll() * 0.1f);
            }
 
 
            float get_hipyawpitch_RAD() {
                return static_cast<float>(target_hipyawpitch()) * RAD_OVER_DEG_10;
            }
            
            float get_hiproll_RAD() {
                return static_cast<float>(target_hiproll()) * RAD_OVER_DEG_10;
            }
        
            float get_hippitch_RAD() {
                return static_cast<float>(target_hippitch()) * RAD_OVER_DEG_10;
            }
        
            float get_kneepitch_RAD() {
                return static_cast<float>(target_kneepitch()) * RAD_OVER_DEG_10;
            }
            
            float get_anklepitch_RAD() {
                return static_cast<float>(target_anklepitch()) * RAD_OVER_DEG_10;
            }
 
            float get_ankleroll_RAD() {
                return static_cast<float>(target_ankleroll()) * RAD_OVER_DEG_10;
            }
 
            float get_hipyawpitchstiffness() {
                return static_cast<float>(target_hipyawpitchstiffness()) * 0.01f;
            }
            
            float get_hiprollstiffness() {
                return static_cast<float>(target_hiprollstiffness()) * 0.01f;
            }
        
            float get_hippitchstiffness() {
                return static_cast<float>(target_hippitchstiffness()) * 0.01f;
            }
        
            float get_kneepitchstiffness() {
                return static_cast<float>(target_kneepitchstiffness()) * 0.01f;
            }
            
            float get_anklepitchstiffness() {
                return static_cast<float>(target_anklepitchstiffness()) * 0.01f;
            }
 
            float get_anklerollstiffness() {
                return static_cast<float>(target_anklerollstiffness()) * 0.01f;
            }
 
//MARK: WHITEBOAR POSTER STRING CONVERSION & Description
        
            std::string description() const
            {
                std::stringstream ss;
                ss << static_cast<int>(target_hipyawpitch()) << " | "
                << static_cast<int>(target_hiproll()) << " | "
                << static_cast<int>(target_hippitch()) << " | "
                << static_cast<int>(target_kneepitch()) << " | "
                << static_cast<int>(target_anklepitch()) << " | "
                << static_cast<int>(target_ankleroll()) << " | "
                << static_cast<int>(target_hipyawpitchstiffness()) << " | "
                << static_cast<int>(target_hiprollstiffness()) << " | "
                << static_cast<int>(target_hippitchstiffness()) << " | "
                << static_cast<int>(target_kneepitchstiffness()) << " | "
                << static_cast<int>(target_anklepitchstiffness()) << " | "
                << static_cast<int>(target_anklerollstiffness()) << " | "
                << static_cast<int>(target_hipyawpitch_active()) << " | "
                << static_cast<int>(target_hiproll_active()) << " | "
                << static_cast<int>(target_hippitch_active()) << " | "
                << static_cast<int>(target_kneepitch_active()) << " | "
                << static_cast<int>(target_anklepitch_active()) << " | "
                << static_cast<int>(target_ankleroll_active()) << " | "
                << static_cast<int>(target_movement_time()) << " | "
                << static_cast<int>(target_pliability()) << " | "
                << static_cast<int>(target_leg_at_goal()) << " | "
                << static_cast<int>(target_leg_stop());
                return ss.str();
            }
 
 
#ifdef WHITEBOARD_POSTER_STRING_CONVERSION
            HalLegTarget(const std::string &str) { from_string(str); }
 
            void from_string(const std::string &str)
            {
                fprintf(stderr, "NYI - Have it back: %s\n", const_cast<char *>(str.c_str()));
            }
 
#endif // WHITEBOARD_POSTER_STRING_CONVERSION
        };
}
 
#endif //HalLegTarget_DEFINED