wb_hal_legtarget.h

Go to the documentation of this file.

 
#ifndef wb_hal_legtarget_h
#define wb_hal_legtarget_h
 
#include <gu_util.h>
#include <stdint.h>
 
#define LEFT_LEG 0
#define RIGHT_LEG 1
 
#define HIP_YAWPITCH_LIMIT_BACK_DEG -65.62f
#define HIP_YAWPITCH_LIMIT_FRONT_DEG 42.44f
 
#define LEFT_HIP_ROLL_OUT_DEG 45.29f
#define LEFT_HIP_ROLL_IN_DEG -21.74f
#define LEFT_HIP_PITCH_FRONT_DEG -88.00f
#define LEFT_HIP_PITCH_BACK_DEG 27.73f
#define LEFT_KNEE_PITCH_FRONT_DEG -5.29f
#define LEFT_KNEE_PITCH_BACK_DEG 121.04f
#define LEFT_ANKLE_PITCH_FRONT_DEG -68.15f
#define LEFT_ANKLE_PITCH_BACK_DEG 52.86f
#define LEFT_ANKLE_ROLL_OUT_DEG 44.06f
#define LEFT_ANKLE_ROLL_IN_DEG -22.79f
 
#define RIGHT_HIP_ROLL_OUT_DEG -45.29f
#define RIGHT_HIP_ROLL_IN_DEG 21.74f
#define RIGHT_HIP_PITCH_FRONT_DEG -88.00f
#define RIGHT_HIP_PITCH_BACK_DEG 27.73f
#define RIGHT_KNEE_PITCH_FRONT_DEG -5.90f
#define RIGHT_KNEE_PITCH_BACK_DEG 121.47f
#define RIGHT_ANKLE_PITCH_FRONT_DEG -67.97f
#define RIGHT_ANKLE_PITCH_BACK_DEG 53.40f
#define RIGHT_ANKLE_ROLL_OUT_DEG -44.06f
#define RIGHT_ANKLE_ROLL_IN_DEG 22.80f
 
#define HIP_YAWPITCH_LIMIT_BACK_RAD -1.145303f
#define HIP_YAWPITCH_LIMIT_FRONT_RAD 0.740810f
 
#define LEFT_HIP_ROLL_OUT_RAD 0.790477f
#define LEFT_HIP_ROLL_IN_RAD -0.379472f
#define LEFT_HIP_PITCH_FRONT_RAD -1.535889f
#define LEFT_HIP_PITCH_BACK_RAD 0.484090f
#define LEFT_KNEE_PITCH_FRONT_RAD -0.092346f
#define LEFT_KNEE_PITCH_BACK_RAD 2.112528f
#define LEFT_ANKLE_PITCH_FRONT_RAD -1.189516f
#define LEFT_ANKLE_PITCH_BACK_RAD 0.922747f
#define LEFT_ANKLE_ROLL_OUT_RAD 0.769001f
#define LEFT_ANKLE_ROLL_IN_RAD -0.397880f
 
#define RIGHT_HIP_ROLL_OUT_RAD -0.790477f
#define RIGHT_HIP_ROLL_IN_RAD 0.379472f
#define RIGHT_HIP_PITCH_FRONT_RAD -1.535889f
#define RIGHT_HIP_PITCH_BACK_RAD 0.484090f
#define RIGHT_KNEE_PITCH_FRONT_RAD -0.103083f
#define RIGHT_KNEE_PITCH_BACK_RAD 2.120198f
#define RIGHT_ANKLE_PITCH_FRONT_RAD -1.186448f
#define RIGHT_ANKLE_PITCH_BACK_RAD 0.932056f
#define RIGHT_ANKLE_ROLL_OUT_RAD -0.768992f
#define RIGHT_ANKLE_ROLL_IN_RAD 0.397935f
 
struct wb_hal_legtarget
{
    PROPERTY(int32_t, target_movement_time)
 
    
    PROPERTY(int16_t, target_hipyawpitch)
 
    
    PROPERTY(int16_t, target_hiproll)
 
    PROPERTY(int16_t, target_hippitch)
 
    PROPERTY(int16_t, target_kneepitch)
 
    PROPERTY(int16_t, target_anklepitch)
 
    PROPERTY(int16_t, target_ankleroll)
 
    PROPERTY(uint8_t, target_leg)
 
    PROPERTY(uint8_t, target_hipyawpitchstiffness)
 
    PROPERTY(uint8_t, target_hiprollstiffness)
 
    PROPERTY(uint8_t, target_hippitchstiffness)
 
    PROPERTY(uint8_t, target_kneepitchstiffness)
 
    PROPERTY(uint8_t, target_anklepitchstiffness)
 
    PROPERTY(uint8_t, target_anklerollstiffness)
 
    PROPERTY(uint8_t, target_pliability)
 
    BIT_PROPERTY(target_hipyawpitch_active)
 
    BIT_PROPERTY(target_hiproll_active)
 
    BIT_PROPERTY(target_hippitch_active)
 
    BIT_PROPERTY(target_kneepitch_active)
 
    BIT_PROPERTY(target_anklepitch_active)
    
    BIT_PROPERTY(target_ankleroll_active)
    
    BIT_PROPERTY(target_leg_at_goal)
    
    BIT_PROPERTY(target_leg_stop)
    
    
 
#ifdef __cplusplus
    wb_hal_legtarget(uint8_t leg = LEFT_LEG,
                     int16_t hipyawpitch = 0,
                     int16_t hiproll = 0,
                     int16_t hippitch = 0,
                     int16_t kneepitch = 0,
                     int16_t anklepitch = 0,
                     int16_t ankleroll = 0,
                     uint8_t hipyawpitchstiffness = 0,
                     uint8_t hiprollstiffness = 0,
                     uint8_t hippitchstiffness = 0,
                     uint8_t kneepitchstiffness = 0,
                     uint8_t anklepitchstiffness = 0,
                     uint8_t anklerollstiffness = 0,
                     bool hipyawpitch_active = true,
                     bool hiproll_active = true,
                     bool hippitch_active = true,
                     bool kneepitch_active = true,
                     bool anklepitch_active = true,
                     bool ankleroll_active = true,
                     int32_t movement_time = INT_MAX, // Ensures sudden movements do not take place
                     uint8_t pliability = 0,
                     bool leg_at_goal = false,
                     bool leg_stop = false)
    {
        set_target_leg(leg);
        set_target_hipyawpitch(hipyawpitch);
        set_target_hiproll(hiproll);
        set_target_hippitch(hippitch);
        set_target_kneepitch(kneepitch);
        set_target_anklepitch(anklepitch);
        set_target_ankleroll(ankleroll);
        set_target_hipyawpitchstiffness(hipyawpitchstiffness);
        set_target_hiprollstiffness(hiprollstiffness);
        set_target_hippitchstiffness(hippitchstiffness);
        set_target_kneepitchstiffness(kneepitchstiffness);
        set_target_anklepitchstiffness(anklepitchstiffness);
        set_target_anklerollstiffness(anklerollstiffness);
        set_target_hipyawpitch_active(hipyawpitch_active);
        set_target_hiproll_active(hiproll_active);
        set_target_hippitch_active(hippitch_active);
        set_target_kneepitch_active(kneepitch_active);
        set_target_anklepitch_active(anklepitch_active);
        set_target_ankleroll_active(ankleroll_active);
        set_target_movement_time(movement_time);
        set_target_pliability(pliability);
        set_target_leg_at_goal(leg_at_goal);
        set_target_leg_stop(leg_stop);
    }
 
    wb_hal_legtarget(const wb_hal_legtarget &other)
    {
        set_target_leg(other.target_leg());
        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());
        set_target_leg_at_goal(other.target_leg_at_goal());
        set_target_leg_stop(other.target_leg_stop());
    }
    
    wb_hal_legtarget &operator = (const wb_hal_legtarget &other)
    {
        set_target_leg(other.target_leg());
        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());
        set_target_leg_at_goal(other.target_leg_at_goal());
        set_target_leg_stop(other.target_leg_stop());
        return *this;
    }
 
    inline bool operator == (const wb_hal_legtarget &rhs)
    {
        if (
            target_leg() == rhs.target_leg()
            && target_hipyawpitch() == rhs.target_hipyawpitch()
            && target_hiproll() == rhs.target_hiproll()
            && target_hippitch() == rhs.target_hippitch()
            && target_kneepitch() == rhs.target_kneepitch()
            && target_anklepitch() == rhs.target_anklepitch()
            && target_ankleroll() == rhs.target_ankleroll()
            && target_hipyawpitchstiffness() == rhs.target_hipyawpitchstiffness()
            && target_hiprollstiffness() == rhs.target_hiprollstiffness()
            && target_hippitchstiffness() == rhs.target_hippitchstiffness()
            && target_kneepitchstiffness() == rhs.target_kneepitchstiffness()
            && target_anklepitchstiffness() == rhs.target_anklepitchstiffness()
            && target_anklerollstiffness() == rhs.target_anklerollstiffness()
            && target_hipyawpitch_active() == rhs.target_hipyawpitch_active()
            && target_hiproll_active() == rhs.target_hiproll_active()
            && target_hippitch_active() == rhs.target_hippitch_active()
            && target_kneepitch_active() == rhs.target_kneepitch_active()
            && target_anklepitch_active() == rhs.target_anklepitch_active()
            && target_ankleroll_active() == rhs.target_ankleroll_active()
            && target_movement_time() == rhs.target_movement_time()
            && target_pliability() == rhs.target_pliability()
            && target_leg_at_goal() == rhs.target_leg_at_goal()
            && target_leg_stop() == rhs.target_leg_stop()
           )
        {
            return true;
        } else {
            return false;
        }
    }
    
    inline bool operator != (const wb_hal_legtarget &rhs)
    {
        return !((*this) == rhs);
    }
 
 
#endif // __cplusplus
};
 
 
 
#endif //wb_hal_legtarget_h