wb_hal_armtarget.h

Go to the documentation of this file.

 
#ifndef wb_hal_armtarget_h
#define wb_hal_armtarget_h
 
#include <gu_util.h>
#include <stdint.h>
 
#define LEFT_ARM 0
#define RIGHT_ARM 1
 
#define SHOULDER_PITCH_LIMIT_DOWN_DEG 119.5f
#define SHOULDER_PITCH_LIMIT_UP_DEG -SHOULDER_PITCH_LIMIT_DOWN_DEG
 
#define LEFT_SHOULDER_ROLL_OUT_DEG 76.0f
#define LEFT_SHOULDER_ROLL_IN_DEG -18.0f
#define RIGHT_SHOULDER_ROLL_OUT_DEG -LEFT_SHOULDER_ROLL_OUT_DEG
#define RIGHT_SHOULDER_ROLL_IN_DEG -LEFT_SHOULDER_ROLL_IN_DEG
 
#define LEFT_ELBOW_ROLL_STRAIGHT_DEG -2.0f
#define LEFT_ELBOW_ROLL_BENT_DEG -88.5f
#define RIGHT_ELBOW_ROLL_STRAIGHT_DEG -LEFT_ELBOW_ROLL_STRAIGHT_DEG
#define RIGHT_ELBOW_ROLL_BENT_DEG -LEFT_ELBOW_ROLL_BENT_DEG
 
#define LEFT_ELBOW_YAW_RIGHT_DEG 119.5f
#define LEFT_ELBOW_YAW_LEFT_DEG -LEFT_ELBOW_YAW_RIGHT_DEG
#define RIGHT_ELBOW_YAW_RIGHT_DEG LEFT_ELBOW_YAW_RIGHT_DEG
#define RIGHT_ELBOW_YAW_LEFT_DEG -LEFT_ELBOW_YAW_RIGHT_DEG
 
#define LEFT_WRIST_YAW_RIGHT_DEG 104.5f
#define LEFT_WRIST_YAW_LEFT_DEG -LEFT_WRIST_YAW_RIGHT_DEG
#define RIGHT_WRIST_YAW_RIGHT_DEG LEFT_WRIST_YAW_RIGHT_DEG
#define RIGHT_WRIST_YAW_LEFT_DEG -LEFT_WRIST_YAW_RIGHT_DEG
 
#define SHOULDER_PITCH_LIMIT_DOWN_RAD 2.0857f
#define SHOULDER_PITCH_LIMIT_UP_RAD -SHOULDER_PITCH_LIMIT_DOWN_RAD
 
#define LEFT_SHOULDER_ROLL_OUT_RAD 1.3265f
#define LEFT_SHOULDER_ROLL_IN_RAD -0.3142f
#define RIGHT_SHOULDER_ROLL_OUT_RAD -LEFT_SHOULDER_ROLL_OUT_RAD
#define RIGHT_SHOULDER_ROLL_IN_RAD -LEFT_SHOULDER_ROLL_IN_RAD
 
#define LEFT_ELBOW_ROLL_STRAIGHT_RAD -0.0349f
#define LEFT_ELBOW_ROLL_BENT_RAD -1.5446f
#define RIGHT_ELBOW_ROLL_STRAIGHT_RAD -LEFT_ELBOW_ROLL_STRAIGHT_RAD
#define RIGHT_ELBOW_ROLL_BENT_RAD -LEFT_ELBOW_ROLL_BENT_RAD
 
#define LEFT_ELBOW_YAW_RIGHT_RAD 2.0857f
#define LEFT_ELBOW_YAW_LEFT_RAD -LEFT_ELBOW_YAW_RIGHT_RAD
#define RIGHT_ELBOW_YAW_RIGHT_RAD LEFT_ELBOW_YAW_RIGHT_RAD
#define RIGHT_ELBOW_YAW_LEFT_RAD -LEFT_ELBOW_YAW_RIGHT_RAD
 
#define LEFT_WRIST_YAW_RIGHT_RAD 1.8238f
#define LEFT_WRIST_YAW_LEFT_RAD -LEFT_WRIST_YAW_RIGHT_RAD
#define RIGHT_WRIST_YAW_RIGHT_RAD LEFT_WRIST_YAW_RIGHT_RAD
#define RIGHT_WRIST_YAW_LEFT_RAD -LEFT_WRIST_YAW_RIGHT_RAD
 
struct wb_hal_armtarget
{
    PROPERTY(int32_t, target_movement_time)
 
    
    PROPERTY(int16_t, target_shoulderpitch)
 
    
    PROPERTY(int16_t, target_shoulderroll)
 
    PROPERTY(int16_t, target_elbowroll)
 
    PROPERTY(int16_t, target_elbowyaw)
 
    PROPERTY(int16_t, target_wristyaw)
 
    PROPERTY(uint8_t, target_arm)
 
    PROPERTY(uint8_t, target_hand)
 
    PROPERTY(uint8_t, target_shoulderpitchstiffness)
 
    PROPERTY(uint8_t, target_shoulderrollstiffness)
 
    PROPERTY(uint8_t, target_elbowrollstiffness)
 
    PROPERTY(uint8_t, target_elbowyawstiffness)
 
    PROPERTY(uint8_t, target_wristyawstiffness)
 
    PROPERTY(uint8_t, target_handstiffness)
 
    PROPERTY(uint8_t, target_pliability)
 
    BIT_PROPERTY(target_shoulderpitch_active)
 
    BIT_PROPERTY(target_shoulderroll_active)
 
    BIT_PROPERTY(target_elbowroll_active)
 
    BIT_PROPERTY(target_elbowyaw_active)
 
    BIT_PROPERTY(target_wrist_active)
    
    BIT_PROPERTY(target_hand_active)
    
    BIT_PROPERTY(target_arm_at_goal)
    
    BIT_PROPERTY(target_arm_stop)
    
    
 
#ifdef __cplusplus
    wb_hal_armtarget(uint8_t arm = LEFT_ARM,
                     int16_t shoulderpitch = 0,
                     int16_t shoulderroll = 0,
                     int16_t elbowroll = 0,
                     int16_t elbowyaw = 0,
                     int16_t wristyaw = 0,
                     uint8_t hand = 0,
                     uint8_t shoulderpitchstiffness = 0,
                     uint8_t shoulderrollstiffness = 0,
                     uint8_t elbowrollstiffness = 0,
                     uint8_t elbowyawstiffness = 0,
                     uint8_t wristyawstiffness = 0,
                     uint8_t handstiffness = 0,
                     bool shoulderpitch_active = true,
                     bool shoulderroll_active = true,
                     bool elbowroll_active = true,
                     bool elbowyaw_active = true,
                     bool wrist_active = true,
                     bool hand_active = true,
                     int32_t movement_time = INT_MAX, // Ensures sudden movements do not take place
                     uint8_t pliability = 0,
                     bool arm_at_goal = false,
                     bool arm_stop = false)
    {
        set_target_arm(arm);
        set_target_shoulderpitch(shoulderpitch);
        set_target_shoulderroll(shoulderroll);
        set_target_elbowroll(elbowroll);
        set_target_elbowyaw(elbowyaw);
        set_target_wristyaw(wristyaw);
        set_target_hand(hand);
        set_target_shoulderpitchstiffness(shoulderpitchstiffness);
        set_target_shoulderrollstiffness(shoulderrollstiffness);
        set_target_elbowrollstiffness(elbowrollstiffness);
        set_target_elbowyawstiffness(elbowyawstiffness);
        set_target_wristyawstiffness(wristyawstiffness);
        set_target_handstiffness(handstiffness);
        set_target_shoulderpitch_active(shoulderpitch_active);
        set_target_shoulderroll_active(shoulderroll_active);
        set_target_elbowroll_active(elbowroll_active);
        set_target_elbowyaw_active(elbowyaw_active);
        set_target_wrist_active(wrist_active);
        set_target_hand_active(hand_active);
        set_target_movement_time(movement_time);
        set_target_pliability(pliability);
        set_target_arm_at_goal(arm_at_goal);
        set_target_arm_stop(arm_stop);
    }
 
    wb_hal_armtarget(const wb_hal_armtarget &other)
    {
        set_target_arm(other.target_arm());
        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());
        set_target_arm_at_goal(other.target_arm_at_goal());
        set_target_arm_stop(other.target_arm_stop());
    }
    
    wb_hal_armtarget &operator = (const wb_hal_armtarget &other)
    {
        set_target_arm(other.target_arm());
        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());
        set_target_arm_at_goal(other.target_arm_at_goal());
        set_target_arm_stop(other.target_arm_stop());
        return *this;
    }
 
    inline bool operator == (const wb_hal_armtarget &rhs)
    {
        if (
            target_arm() == rhs.target_arm()
            && target_shoulderpitch() == rhs.target_shoulderpitch()
            && target_shoulderroll() == rhs.target_shoulderroll()
            && target_elbowroll() == rhs.target_elbowroll()
            && target_elbowyaw() == rhs.target_elbowyaw()
            && target_wristyaw() == rhs.target_wristyaw()
            && target_hand() == rhs.target_hand()
            && target_shoulderpitchstiffness() == rhs.target_shoulderpitchstiffness()
            && target_shoulderrollstiffness() == rhs.target_shoulderrollstiffness()
            && target_elbowrollstiffness() == rhs.target_elbowrollstiffness()
            && target_elbowyawstiffness() == rhs.target_elbowyawstiffness()
            && target_wristyawstiffness() == rhs.target_wristyawstiffness()
            && target_handstiffness() == rhs.target_handstiffness()
            && target_shoulderpitch_active() == rhs.target_shoulderpitch_active()
            && target_shoulderroll_active() == rhs.target_shoulderroll_active()
            && target_elbowroll_active() == rhs.target_elbowroll_active()
            && target_elbowyaw_active() == rhs.target_elbowyaw_active()
            && target_wrist_active() == rhs.target_wrist_active()
            && target_hand_active() == rhs.target_hand_active()
            && target_movement_time() == rhs.target_movement_time()
            && target_pliability() == rhs.target_pliability()
            && target_arm_at_goal() == rhs.target_arm_at_goal()
            && target_arm_stop() == rhs.target_arm_stop()
           )
        {
            return true;
        } else {
            return false;
        }
    }
    
    inline bool operator != (const wb_hal_armtarget &rhs)
    {
        return !((*this) == rhs);
    }
 
 
#endif // __cplusplus
};
 
 
 
#endif //wb_hal_armtarget_h