gait_inverse_pose.h

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

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

// Includes
#include <gait/util/gait_common_pose.h>
#include <Eigen/Geometry>

// 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 InverseLegPose
    {
        //
        // Constructor
        //

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

        inline void reset(bool left = true)
        {
            cld.reset(left);
            setPose(Eigen::Vector3d::Zero(), Eigen::Quaterniond::Identity());
        }

        //
        // Set functions
        //

        inline void setPose(const Eigen::Vector3d& footPosition, const Eigen::Quaterniond& footRotation = Eigen::Quaterniond::Identity())
        {
            footPos = footPosition;
            footRot = footRotation;
        }

        inline void setPoseMirrored(const Eigen::Vector3d& footPosition, const Eigen::Quaterniond& footRotation = Eigen::Quaterniond::Identity())
        {
            footPos = footPosition;
            footRot = footRotation;
            if(!cld.isLeft)
            {
                footPos.y() = -footPos.y(); // Flip the position about the XZ plane
                footRot.x() = -footRot.x(); // Mirror the orientation about the XZ plane
                footRot.z() = -footRot.z(); // ...
            }
        }

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

        //
        // Pose conversion functions
        //

        void setFromJointPose(const JointLegPose& pose);

        void setFromAbstractPose(const AbstractLegPose& 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
        Eigen::Vector3d footPos;    
        Eigen::Quaterniond footRot; 
    };

    struct InverseArmPose
    {
        //
        // Constructor
        //

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

        inline void reset(bool left = true)
        {
            cad.reset(left);
            setPose(/* TODO */);
        }

        //
        // Set functions
        //

        inline void setPose(/* TODO */)
        {
        }

        inline void setPoseMirrored(/* TODO */)
        {
        }

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

        //
        // Pose conversion functions
        //

        void setFromJointPose(const JointArmPose& pose);

        void setFromAbstractPose(const AbstractArmPose& 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
        /* TODO */
    };

    struct InversePose
    {
        //
        // Constructor
        //

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

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

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

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

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

        //
        // Data members
        //

        // Inverse limb pose structs
        InverseLegPose leftLeg;  
        InverseLegPose rightLeg; 
        InverseArmPose leftArm;  
        InverseArmPose rightArm; 
    };
}

#endif /* GAIT_INVERSE_POSE_H */
// EOF