HAL_ArmTarget.hpp

Go to the documentation of this file.

 
#ifndef HalArmTarget_DEFINED
#define HalArmTarget_DEFINED
 
#include "wb_hal_armtarget.h"
//#include <cmath>
#include <sstream>
 
//#ifndef M_PIF
//#define M_PIF static_cast<float>(M_PI)
//#endif  // M_PIF
 
namespace guWhiteboard
{
    class HalArmTarget : public wb_hal_armtarget
    {
 
        public:
//            HalArmTarget(const uint8_t &target_arm = LEFT_ARM): wb_hal_armtarget(target_arm) {}
        
            HalArmTarget(const uint8_t &target_arm = LEFT_ARM,
                         const float shoulderpitch = 0,
                         const float shoulderroll = 0,
                         const float elbowroll = 0,
                         const float elbowyaw = 0,
                         const float wristyaw = 0,
                         const float hand_PCT = 0,
                         const float shoulderpitchstiffness = 0,
                         const float shoulderrollstiffness = 0,
                         const float elbowrollstiffness = 0,
                         const float elbowyawstiffness = 0,
                         const float wristyawstiffness = 0,
                         const float handstiffness = 0,
                         const bool  shoulderpitch_active = true,
                         const bool  shoulderroll_active = true,
                         const bool  elbowroll_active = true,
                         const bool  elbowyaw_active = true,
                         const bool  wrist_active = true,
                         const bool  hand_active = true,
                         const int32_t movement_time = 1000000,
                         const uint8_t pliability = 10,
                         const bool  arm_stop = false,
                         const bool inRadians = false)
            : wb_hal_armtarget(target_arm)
            {
               if (inRadians) {
                   setPose_Rad(shoulderpitch, shoulderroll,
                               elbowroll, elbowyaw,
                               wristyaw, hand_PCT);
               } else {
                   setPose_Deg(shoulderpitch, shoulderroll,
                               elbowroll, elbowyaw,
                               wristyaw, hand_PCT);
               }
               set_target_movement_time(movement_time);
               set_target_arm_stop(arm_stop);
               set_shoulderpitchstiffness(shoulderpitchstiffness);
               set_shoulderrollstiffness(shoulderrollstiffness);
               set_elbowrollstiffness(elbowrollstiffness);
               set_elbowyawstiffness(elbowyawstiffness);
               set_wristyawstiffness(wristyawstiffness);
               set_handstiffness(handstiffness);
               setArmPliability(shoulderpitch_active, shoulderroll_active, elbowroll_active,
                                elbowyaw_active, wrist_active, hand_active);
               set_target_pliability(pliability);
            }
 
//            HalArmTarget(const uint8_t &target_arm = LEFT_ARM,
//                          const float shoulderpitch_DEG = 0,
//                          const float shoulderroll_DEG = 0,
//                          const float elbowroll_DEG = 0,
//                          const float elbowyaw_DEG = 0,
//                          const float wristyaw_DEG = 0,
//                          const float hand_PCT = 0,
//                          const float shoulderpitchstiffness = 0,
//                          const float target_shoulderrollstiffness = 0,
//                          const float target_elbowrollstiffness = 0,
//                          const float target_elbowyawstiffness = 0,
//                          const float target_wristyawstiffness = 0,
//                          const bool  shoulderpitch_active = true,
//                          const bool  shoulderroll_active = true,
//                          const bool  elbowroll_active = true,
//                          const bool  elbowyaw_active = true,
//                          const bool  wrist_active = true,
//                          const bool  hand_active = true,
//                          const int32_t movement_time = 1000000,
//                          set_target_pliability(pliability);
//            : wb_hal_armtarget(target_arm)
//            {
//               setPose_Deg(shoulderpitch_DEG, shoulderroll_DEG,
//                           elbowroll_DEG, elbowyaw_DEG,
//                           wristyaw_DEG, hand_PCT);
//               set_target_movement_time(movement_time);
//               set_shoulderpitchstiffness(shoulderpitchstiffness);
//               set_shoulderrollstiffness(shoulderrollstiffness);
//               set_elbowrollstiffness(elbowrollstiffness);
//               set_elbowyawstiffness(elbowyawstiffness);
//               set_wristyawstiffness(wristyawstiffness);
//               set_handstiffness(handstiffness);
//               setArmPliability(shoulderpitch_active, shoulderroll_active, elbowroll_active,
//                                elbowyaw_active, wrist_active, hand_active);
//            }
 
//MARK: Arm - General
            void tieToArm(uint8_t arm)
            {
                set_target_arm(arm);
            }
 
//MARK: Arm - Movements
 
            void goToWithTime_Rad(float shoulderpitch,
                                  float shoulderroll,
                                  float elbowroll,
                                  float elbowyaw,
                                  float wristyaw,
                                  float hand,
                                  int32_t time = INT_MAX)
            {
                setPose_Rad(shoulderpitch, shoulderroll,
                            elbowroll, elbowyaw,
                            wristyaw, hand);
                set_target_movement_time(time);
            }
            
            void goToWithTime_Deg(float shoulderpitch,
                                  float shoulderroll,
                                  float elbowroll,
                                  float elbowyaw,
                                  float wristyaw,
                                  float hand,
                                  int32_t time = INT_MAX)
            {
                setPose_Deg(shoulderpitch, shoulderroll,
                            elbowroll, elbowyaw,
                            wristyaw, hand);
                set_target_movement_time(time);
            }
        
            void stop() {
                set_target_arm_stop(true);
            }
 
        
            void ready() {
                set_target_arm_stop(false);
            }
 
        
            void isAtGoal(bool goalReached)
            {
                set_target_arm_at_goal(goalReached);
            }
            
            bool atGoal()
            {
                return target_arm_at_goal();
            }
 
            bool atTargetLocation(HalArmTarget status, HalArmTarget tolerance)
            {
                int16_t shoulderpitchMargin = static_cast<int16_t>(abs(target_shoulderpitch() - status.target_shoulderpitch()));
                int16_t shoulderrollMargin = static_cast<int16_t>(abs(target_shoulderroll() - status.target_shoulderroll()));
                int16_t elbowrollMargin = static_cast<int16_t>(abs(target_elbowroll() - status.target_elbowroll()));
                int16_t elbowyawMargin = static_cast<int16_t>(abs(target_elbowyaw() - status.target_elbowyaw()));
                //#ifdef NAO_V3
                //    int16_t wristyawMargin = static_cast<int16_t>(abs(target_wristyaw - status.target_wristyaw()));
                //    uint8_t handMargin = static_cast<uint8_t>(abs(target_hand() - status.target_hand()));
                if (   (shoulderpitchMargin <= tolerance.target_shoulderpitch())
                    && (shoulderrollMargin <= tolerance.target_shoulderroll())
                    && (elbowrollMargin <= tolerance.target_elbowroll())
                    && (elbowyawMargin <= tolerance.target_elbowyaw())
                    //#ifdef NAO_V3
                    //        && (wristyawMargin <= tolerance.target_wristyaw())
                    //        && (handMargin <= tolerance.target_hand())
                    )
                {
                    return true;
                }
                return false;
            }
 
//MARK: Arm - Pose
 
            void setPose_Rad(float shoulderpitch,
                             float shoulderroll,
                             float elbowroll,
                             float elbowyaw,
                             float wristyaw,
                             float hand)
            {
                set_shoulderpitch_RAD(shoulderpitch);
                set_shoulderroll_RAD(shoulderroll);
                set_elbowroll_RAD(elbowroll);
                set_elbowyaw_RAD(elbowyaw);
                set_wristyaw_RAD(wristyaw);
                set_hand(hand);
            }
            
            void setPose_Deg(float shoulderpitch,
                             float shoulderroll,
                             float elbowroll,
                             float elbowyaw,
                             float wristyaw,
                             float hand)
            {
                set_shoulderpitch_DEG(shoulderpitch);
                set_shoulderroll_DEG(shoulderroll);
                set_elbowroll_DEG(elbowroll);
                set_elbowyaw_DEG(elbowyaw);
                set_wristyaw_DEG(wristyaw);
                set_hand(hand);
            }
 
            
            void mirrorArm(const HalArmTarget &other)
            {
                // Roll and Yaw angles need to be mirrored, others just copied.
                set_target_shoulderpitch(other.target_shoulderpitch());
                set_target_shoulderroll(-other.target_shoulderroll());
                set_target_elbowroll(-other.target_elbowroll());
                set_target_elbowyaw(-other.target_elbowyaw());
                set_target_wristyaw(-other.target_wristyaw());
                set_target_hand(other.target_hand());
                set_target_shoulderpitchstiffness(other.target_shoulderpitchstiffness());
                set_target_shoulderrollstiffness(other.target_shoulderrollstiffness());
                set_target_elbowrollstiffness(other.target_elbowrollstiffness());
                set_target_elbowyawstiffness(other.target_elbowyawstiffness());
                set_target_wristyawstiffness(other.target_wristyawstiffness());
                set_target_handstiffness(other.target_handstiffness());
                set_target_shoulderpitch_active(other.target_shoulderpitch_active());
                set_target_shoulderroll_active(other.target_shoulderroll_active());
                set_target_elbowroll_active(other.target_elbowroll_active());
                set_target_elbowyaw_active(other.target_elbowyaw_active());
                set_target_wrist_active(other.target_wrist_active());
                set_target_hand_active(other.target_hand_active());
                set_target_movement_time(other.target_movement_time());
                set_target_pliability(other.target_pliability());
            }
            
            void copyPose(const HalArmTarget &other)
            {
                set_target_shoulderpitch(other.target_shoulderpitch());
                set_target_shoulderroll(other.target_shoulderroll());
                set_target_elbowroll(other.target_elbowroll());
                set_target_elbowyaw(other.target_elbowyaw());
                set_target_wristyaw(other.target_wristyaw());
                set_target_hand(other.target_hand());
            }
            
            void mirrorPose(const HalArmTarget &other)
            {
                set_target_shoulderpitch(other.target_shoulderpitch());
                set_target_shoulderroll(-other.target_shoulderroll());
                set_target_elbowroll(-other.target_elbowroll());
                set_target_elbowyaw(-other.target_elbowyaw());
                set_target_wristyaw(-other.target_wristyaw());
                set_target_hand(other.target_hand());
            }
            
            bool hasSamePose(const HalArmTarget &other)
            {
                if (
                    target_shoulderpitch() == other.target_shoulderpitch()
                    && target_shoulderroll() == other.target_shoulderroll()
                    && target_elbowroll() == other.target_elbowroll()
                    && target_elbowyaw() == other.target_elbowyaw()
                    && target_wristyaw() == other.target_wristyaw()
                    && target_hand() == other.target_hand()
                    )
                {
                    return  true;
                }
                return false;
            }
            
            bool hasSameMirroredPose(const HalArmTarget &other)
            {
                if (
                    target_shoulderpitch() == other.target_shoulderpitch()
                    && target_shoulderroll() == -other.target_shoulderroll()
                    && target_elbowroll() == -other.target_elbowroll()
                    && target_elbowyaw() == -other.target_elbowyaw()
                    && target_wristyaw() == -other.target_wristyaw()
                    && target_hand() == other.target_hand()
                    )
                {
                    return  true;
                }
                return false;
            }
 
//MARK: Arm - Stiffness
 
            void setArmStiffnessMax()
            {
                set_shoulderpitchstiffness(1.0f);
                set_shoulderrollstiffness(1.0f);
                set_elbowrollstiffness(1.0f);
                set_elbowyawstiffness(1.0f);
                set_wristyawstiffness(1.0f);
                set_handstiffness(1.0f);
            }
 
            void setArmStiffnessNormal()
            {
                set_shoulderpitchstiffness(0.6f);
                set_shoulderrollstiffness(0.6f);
                set_elbowrollstiffness(0.6f);
                set_elbowyawstiffness(0.6f);
                set_wristyawstiffness(0.6f);
                set_handstiffness(0.6f);
            }
 
            void setArmStiffness(float stiffness)
            {
                if ((stiffness <= 1.0f) && (stiffness >=0.0f)) {
                    set_shoulderpitchstiffness(stiffness);
                    set_shoulderrollstiffness(stiffness);
                    set_elbowrollstiffness(stiffness);
                    set_elbowyawstiffness(stiffness);
                    set_wristyawstiffness(stiffness);
                    set_handstiffness(stiffness);
                }
            }
 
            void setArmStiffnessOff()
            {
                set_shoulderpitchstiffness(0.0f);
                set_shoulderrollstiffness(0.0f);
                set_elbowrollstiffness(0.0f);
                set_elbowyawstiffness(0.0f);
                set_wristyawstiffness(0.0f);
                set_handstiffness(0.0f);
            }
 
        
            void copyStiffness(const HalArmTarget &other)
            {
                target_shoulderpitchstiffness() = other.target_shoulderpitchstiffness();
                target_shoulderrollstiffness() = other.target_shoulderrollstiffness();
                target_elbowrollstiffness() = other.target_elbowrollstiffness();
                target_elbowyawstiffness() = other.target_elbowyawstiffness();
                target_wristyawstiffness() = other.target_wristyawstiffness();
                target_handstiffness() = other.target_handstiffness();
            }
        
 
 
            bool hasSameStiffness(const HalArmTarget &other)
            {
                if (
                    target_shoulderpitchstiffness() == other.target_shoulderpitchstiffness()
                    && target_shoulderrollstiffness() == other.target_shoulderrollstiffness()
                    && target_elbowrollstiffness() == other.target_elbowrollstiffness()
                    && target_elbowyawstiffness() == other.target_elbowyawstiffness()
                    && target_wristyawstiffness() == other.target_wristyawstiffness()
                    && target_handstiffness() == other.target_handstiffness()
                   )
                {
                    return  true;
                }
                return false;
            }
 
//MARK: Arm - Pliability
                
            void setArmActive()
            {
                set_target_shoulderpitch_active(true);
                set_target_shoulderroll_active(true);
                set_target_elbowroll_active(true);
                set_target_elbowyaw_active(true);
                set_target_wrist_active(true);
                set_target_hand_active(true);
            }
 
            void setArmPassive()
            {
                set_target_shoulderpitch_active(false);
                set_target_shoulderroll_active(false);
                set_target_elbowroll_active(false);
                set_target_elbowyaw_active(false);
                set_target_wrist_active(false);
                set_target_hand_active(false);
            }
            
            
            void setArmPliability(bool shoulderpitch, bool shoulderroll, bool elbowroll,
                                bool elbowyaw, bool wristyaw, bool hand)
            {
                set_target_shoulderpitch_active(shoulderpitch);
                set_target_shoulderroll_active(shoulderroll);
                set_target_elbowroll_active(elbowroll);
                set_target_elbowyaw_active(elbowyaw);
                set_target_wrist_active(wristyaw);
                set_target_hand_active(hand);
            }
 
            bool isArmPassive() {
                return !target_shoulderpitch_active() || !target_shoulderroll_active() || !target_elbowroll_active() || !target_elbowyaw_active() || !target_wrist_active() || !target_hand_active();
            }
 
            bool isArmAllPassive() {
                return !target_shoulderpitch_active() && !target_shoulderroll_active() && !target_elbowroll_active() && !target_elbowyaw_active() && !target_wrist_active() && !target_hand_active();
            }
        
            bool isArmAllActive() {
                return target_shoulderpitch_active() && target_shoulderroll_active() && target_elbowroll_active() && target_elbowyaw_active() && target_wrist_active() && target_hand_active();
            }
        
//MARK: CUSTOM SETTERS (Converting floats into Integer representations)
            void set_shoulderpitch_DEG(float setting) {
                set_target_shoulderpitch(static_cast<int16_t>(setting * 10.0f));
            }
            
            void set_shoulderroll_DEG(float setting) {
                set_target_shoulderroll(static_cast<int16_t>(setting * 10.0f));
            }
        
            void set_elbowroll_DEG(float setting) {
                set_target_elbowroll(static_cast<int16_t>(setting * 10.0f));
            }
        
            void set_elbowyaw_DEG(float setting) {
                set_target_elbowyaw(static_cast<int16_t>(setting * 10.0f));
            }
            
            void set_wristyaw_DEG(float setting) {
                set_target_wristyaw(static_cast<int16_t>(setting * 10.0f));
            }
 
            void set_shoulderpitch_RAD(float setting) {
                set_target_shoulderpitch(static_cast<int16_t>(setting* DEG_OVER_RAD_10));
            }
            
            void set_shoulderroll_RAD(float setting) {
                set_target_shoulderroll(static_cast<int16_t>(setting * DEG_OVER_RAD_10));
            }
        
            void set_elbowroll_RAD(float setting) {
                set_target_elbowroll(static_cast<int16_t>(setting * DEG_OVER_RAD_10));
            }
        
            void set_elbowyaw_RAD(float setting) {
                set_target_elbowyaw(static_cast<int16_t>(setting * DEG_OVER_RAD_10));
            }
            
            void set_wristyaw_RAD(float setting) {
                set_target_wristyaw(static_cast<int16_t>(setting * DEG_OVER_RAD_10));
            }
        
            void set_hand(float setting) {
                set_target_hand(static_cast<uint8_t>(setting * 100.0f));
            }
 
            void set_shoulderpitchstiffness(float setting) {
                set_target_shoulderpitchstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
            
            void set_shoulderrollstiffness(float setting) {
                set_target_shoulderrollstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
        
            void set_elbowrollstiffness(float setting) {
                set_target_elbowrollstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
        
            void set_elbowyawstiffness(float setting) {
                set_target_elbowyawstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
            
            void set_wristyawstiffness(float setting) {
                set_target_wristyawstiffness(static_cast<uint8_t>(setting * 100.0f));
            }
 
            void set_handstiffness(float setting) {
                set_target_handstiffness(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_shoulderpitch_DEG() {
                return static_cast<float>(target_shoulderpitch() * 0.1f);
            }
            
            float get_shoulderroll_DEG() {
                return static_cast<float>(target_shoulderroll() * 0.1f);
            }
        
            float get_elbowroll_DEG() {
                return static_cast<float>(target_elbowroll() * 0.1f);
            }
        
            float get_elbowyaw_DEG() {
                return static_cast<float>(target_elbowyaw() * 0.1f);
            }
            
            float get_wristyaw_DEG() {
                return static_cast<float>(target_wristyaw() * 0.1f);
            }
 
            float get_shoulderpitch_RAD() {
                return static_cast<float>(target_shoulderpitch()) * RAD_OVER_DEG_10;
            }
            
            float get_shoulderroll_RAD() {
                return static_cast<float>(target_shoulderroll()) * RAD_OVER_DEG_10;
            }
        
            float get_elbowroll_RAD() {
                return static_cast<float>(target_elbowroll()) * RAD_OVER_DEG_10;
            }
        
            float get_elbowyaw_RAD() {
                return static_cast<float>(target_elbowyaw()) * RAD_OVER_DEG_10;
            }
            
            float get_wristyaw_RAD() {
                return static_cast<float>(target_wristyaw()) * RAD_OVER_DEG_10;
            }
 
            float get_hand() {
                return static_cast<float>(target_hand()) * 0.01f;
            }
 
            float get_shoulderpitchstiffness() {
                return static_cast<float>(target_shoulderpitchstiffness()) * 0.01f;
            }
            
            float get_shoulderrollstiffness() {
                return static_cast<float>(target_shoulderrollstiffness()) * 0.01f;
            }
        
            float get_elbowrollstiffness() {
                return static_cast<float>(target_elbowrollstiffness()) * 0.01f;
            }
        
            float get_elbowyawstiffness() {
                return static_cast<float>(target_elbowyawstiffness()) * 0.01f;
            }
            
            float get_wristyawstiffness() {
                return static_cast<float>(target_wristyawstiffness()) * 0.01f;
            }
 
            float get_handstiffness() {
                return static_cast<float>(target_handstiffness()) * 0.01f;
            }
 
//MARK: WHITEBOAR POSTER STRING CONVERSION & Description
        
            std::string description() const
            {
                std::stringstream ss;
                ss << static_cast<int>(target_shoulderpitch()) << " | "
                << static_cast<int>(target_shoulderroll()) << " | "
                << static_cast<int>(target_elbowroll()) << " | "
                << static_cast<int>(target_elbowyaw()) << " | "
                << static_cast<int>(target_wristyaw()) << " | "
                << static_cast<int>(target_hand()) << " | "
                << static_cast<int>(target_shoulderpitchstiffness()) << " | "
                << static_cast<int>(target_shoulderrollstiffness()) << " | "
                << static_cast<int>(target_elbowrollstiffness()) << " | "
                << static_cast<int>(target_elbowyawstiffness()) << " | "
                << static_cast<int>(target_wristyawstiffness()) << " | "
                << static_cast<int>(target_handstiffness()) << " | "
                << static_cast<int>(target_shoulderpitch_active()) << " | "
                << static_cast<int>(target_shoulderroll_active()) << " | "
                << static_cast<int>(target_elbowroll_active()) << " | "
                << static_cast<int>(target_elbowyaw_active()) << " | "
                << static_cast<int>(target_wrist_active()) << " | "
                << static_cast<int>(target_hand_active()) << " | "
                << static_cast<int>(target_movement_time()) << " | "
                << static_cast<int>(target_pliability()) << " | "
                << static_cast<int>(target_arm_at_goal()) << " | "
                << static_cast<int>(target_arm_stop());
                return ss.str();
            }
 
 
#ifdef WHITEBOARD_POSTER_STRING_CONVERSION
            HalArmTarget(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 //HAL_HeadTarget_DEFINED