gait_joint_pose.h

// Gait utility class that provides joint pose representation functionality
// File: gait_joint_pose.h
// Author: Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Ensure header is only included once
#ifndef GAIT_JOINT_POSE_H
#define GAIT_JOINT_POSE_H

// Includes
#include <gait/util/gait_common_pose.h>
#include <gait/gait_common.h>

// Gait namespace
namespace gait
{
    // Class forward declarations
    class JointPose;
    class JointLegPose;
    class JointArmPose;
    class InversePose;
    class InverseLegPose;
    class InverseArmPose;
    class AbstractPose;
    class AbstractLegPose;
    class AbstractArmPose;

    struct JointLegPose
    {
        //
        // Constructor
        //

        explicit JointLegPose(bool left = true) { reset(left); }

        inline void reset(bool left = true)
        {
            cld.reset(left);
            setPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
        }

        //
        // Set functions
        //

        inline void setPose(double hipYaw, double hipRoll, double hipPitch, double kneePitch, double anklePitch, double ankleRoll)
        {
            this->hipYaw = hipYaw;
            this->hipRoll = hipRoll;
            this->hipPitch = hipPitch;
            this->kneePitch = kneePitch;
            this->anklePitch = anklePitch;
            this->ankleRoll = ankleRoll;
        }

        inline void setPoseMirrored(double hipYaw, double hipRoll, double hipPitch, double kneePitch, double anklePitch, double ankleRoll)
        {
            this->hipYaw = (cld.isLeft ? hipYaw : -hipYaw);
            this->hipRoll = (cld.isLeft ? hipRoll : -hipRoll);
            this->hipPitch = hipPitch;
            this->kneePitch = kneePitch;
            this->anklePitch = anklePitch;
            this->ankleRoll = (cld.isLeft ? ankleRoll : -ankleRoll);
        }

        inline void setLinkLength(double length)
        {
            cld.linkLength = length;
        }

        //
        // Pose conversion functions
        //

        void setFromInversePose(const InverseLegPose& pose);

        void setFromAbstractPose(const AbstractLegPose& pose);

        inline void fromJointAngles(double hipYaw, double hipRoll, double hipPitch, double kneePitch, double anklePitch, double ankleRoll)
        {
            this->hipYaw = hipYaw;
            this->hipRoll = hipRoll;
            this->hipPitch = hipPitch;
            this->kneePitch = kneePitch;
            this->anklePitch = anklePitch;
            this->ankleRoll = ankleRoll;
        }

        inline void getJointAngles(double& hipYaw, double& hipRoll, double& hipPitch, double& kneePitch, double& anklePitch, double& ankleRoll) const
        {
            hipYaw = this->hipYaw;
            hipRoll = this->hipRoll;
            hipPitch = this->hipPitch;
            kneePitch = this->kneePitch;
            anklePitch = this->anklePitch;
            ankleRoll = this->ankleRoll;
        }

        //
        // Other functions
        //

        void blendTowards(const JointLegPose& other, double b);

        //
        // Data members
        //

        // Common leg data
        CommonLegData cld; 

        // Leg pose
        double hipYaw;     
        double hipRoll;    
        double hipPitch;   
        double kneePitch;  
        double anklePitch; 
        double ankleRoll;  
    };

    struct JointArmPose
    {
        //
        // Constructor
        //

        explicit JointArmPose(bool left = true) { reset(left); }

        inline void reset(bool left = true)
        {
            cad.reset(left);
            setPose(0.0, 0.0, 0.0);
        }

        //
        // Set functions
        //

        inline void setPose(double shoulderPitch, double shoulderRoll, double elbowPitch)
        {
            this->shoulderPitch = shoulderPitch;
            this->shoulderRoll = shoulderRoll;
            this->elbowPitch = elbowPitch;
        }

        inline void setPoseMirrored(double shoulderPitch, double shoulderRoll, double elbowPitch)
        {
            this->shoulderPitch = shoulderPitch;
            this->shoulderRoll = (cad.isLeft ? shoulderRoll : -shoulderRoll);
            this->elbowPitch = elbowPitch;
        }

        inline void setLinkLength(double length)
        {
            cad.linkLength = length;
        }

        //
        // Pose conversion functions
        //

        void setFromInversePose(const InverseArmPose& pose);

        void setFromAbstractPose(const AbstractArmPose& pose);

        inline void fromJointAngles(double shoulderPitch, double shoulderRoll, double elbowPitch)
        {
            this->shoulderPitch = shoulderPitch;
            this->shoulderRoll = shoulderRoll;
            this->elbowPitch = elbowPitch;
        }

        inline void getJointAngles(double& shoulderPitch, double& shoulderRoll, double& elbowPitch) const
        {
            shoulderPitch = this->shoulderPitch;
            shoulderRoll = this->shoulderRoll;
            elbowPitch = this->elbowPitch;
        }

        //
        // Other functions
        //

        void blendTowards(const JointArmPose& other, double b);

        //
        // Data members
        //

        // Common arm data
        CommonArmData cad;    

        // Arm pose
        double shoulderPitch; 
        double shoulderRoll;  
        double elbowPitch;    
    };

    struct JointPose
    {
        //
        // Constructor
        //

        JointPose() { reset(); }

        inline void reset()
        {
            leftLeg.reset(true);
            rightLeg.reset(false);
            leftArm.reset(true);
            rightArm.reset(false);
        }

        //
        // Set functions
        //

        inline void setLinkLengths(double legLinkLength, double armLinkLength)
        {
            leftLeg.cld.setLinkLength(legLinkLength);
            rightLeg.cld.setLinkLength(legLinkLength);
            leftArm.cad.setLinkLength(armLinkLength);
            rightArm.cad.setLinkLength(armLinkLength);
        }

        //
        // Pose conversion functions
        //

        void setFromInversePose(const InversePose& pose);

        void setFromAbstractPose(const AbstractPose& pose);

        inline void setLegsFromInversePose(const InverseLegPose& left, const InverseLegPose& right)
        {
            leftLeg.setFromInversePose(left);
            rightLeg.setFromInversePose(right);
        }

        inline void setLegsFromAbstractPose(const AbstractLegPose& left, const AbstractLegPose& right)
        {
            leftLeg.setFromAbstractPose(left);
            rightLeg.setFromAbstractPose(right);
        }

        inline void setArmsFromInversePose(const InverseArmPose& left, const InverseArmPose& right)
        {
            leftArm.setFromInversePose(left);
            rightArm.setFromInversePose(right);
        }

        inline void setArmsFromAbstractPose(const AbstractArmPose& left, const AbstractArmPose& right)
        {
            leftArm.setFromAbstractPose(left);
            rightArm.setFromAbstractPose(right);
        }

        //
        // Data interface functions
        //

        void readJointPosArray(const double (&pos)[NUM_JOINTS]);

        void readJointEffortArray(const double (&effort)[NUM_JOINTS]);

        void writeJointPosArray(double (&pos)[NUM_JOINTS]) const;

        void writeJointEffortArray(double (&effort)[NUM_JOINTS]) const;

        //
        // Other functions
        //

        void blendTowards(const JointPose& other, double b);

        //
        // Data members
        //

        // Joint limb pose structs
        JointLegPose leftLeg;  
        JointLegPose rightLeg; 
        JointArmPose leftArm;  
        JointArmPose rightArm; 
    };
}

#endif /* GAIT_JOINT_POSE_H */
// EOF