gait_abstract_pose.h

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

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

// Includes
#include <gait/util/gait_common_pose.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 AbstractLegPose
    {
        //
        // Constructor
        //

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

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

        //
        // Set functions
        //

        inline void setPose(double ext, double angX, double angY, double angZ)
        {
            extension = ext;
            angleX = angX;
            angleY = angY;
            angleZ = angZ;
        }

        inline void setPoseMirrored(double ext, double angX, double angY, double angZ)
        {
            extension = ext;
            angleX = (cld.isLeft ? angX : -angX);
            angleY = angY;
            angleZ = (cld.isLeft ? angZ : -angZ);
        }

        inline void setFootPose(double footAngX, double footAngY)
        {
            footAngleX = footAngX;
            footAngleY = footAngY;
        }

        inline void setFootPoseMirrored(double footAngX, double footAngY)
        {
            footAngleX = (cld.isLeft ? footAngX : -footAngX);
            footAngleY = footAngY;
        }

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

        //
        // Pose conversion functions
        //

        void setFromJointPose(const JointLegPose& pose);

        void setFromInversePose(const InverseLegPose& pose);

        void fromJointAngles(double hipYaw, double hipRoll, double hipPitch, double kneePitch, double anklePitch, double ankleRoll);

        void getJointAngles(double& hipYaw, double& hipRoll, double& hipPitch, double& kneePitch, double& anklePitch, double& ankleRoll) const;

        //
        // Data members
        //

        // Common leg data
        CommonLegData cld; 

        // Leg pose
        double extension;  
        double angleX;     
        double angleY;     
        double angleZ;     

        // Foot pose
        double footAngleX; 
        double footAngleY; 
    };

    struct AbstractArmPose
    {
        //
        // Constructor
        //

        explicit AbstractArmPose(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 ext, double angX, double angY)
        {
            extension = ext;
            angleX = angX;
            angleY = angY;
        }

        inline void setPoseMirrored(double ext, double angX, double angY)
        {
            extension = ext;
            angleX = (cad.isLeft ? angX : -angX);
            angleY = angY;
        }

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

        //
        // Pose conversion functions
        //

        void setFromJointPose(const JointArmPose& pose);

        void setFromInversePose(const InverseArmPose& pose);

        void fromJointAngles(double shoulderPitch, double shoulderRoll, double elbowPitch);

        void getJointAngles(double& shoulderPitch, double& shoulderRoll, double& elbowPitch) const;

        //
        // Data members
        //

        // Common arm data
        CommonArmData cad; 

        // Arm pose
        double extension;  
        double angleX;     
        double angleY;     
    };

    struct AbstractPose
    {
        //
        // Constructor
        //

        AbstractPose() { 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 setFromJointPose(const JointPose& pose);

        void setFromInversePose(const InversePose& pose);

        inline void setLegsFromJointPose(const JointLegPose& left, const JointLegPose& right)
        {
            leftLeg.setFromJointPose(left);
            rightLeg.setFromJointPose(right);
        }

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

        inline void setArmsFromJointPose(const JointArmPose& left, const JointArmPose& right)
        {
            leftArm.setFromJointPose(left);
            rightArm.setFromJointPose(right);
        }

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

        //
        // Data members
        //

        // Abstract limb pose structs
        AbstractLegPose leftLeg;  
        AbstractLegPose rightLeg; 
        AbstractArmPose leftArm;  
        AbstractArmPose rightArm; 
    };
}

#endif /* GAIT_ABSTRACT_POSE_H */
// EOF